diff --git a/robowaiter/behavior_lib/_base/Behavior.py b/robowaiter/behavior_lib/_base/Behavior.py index 92eb9d4..8baf2ca 100644 --- 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', diff --git a/robowaiter/behavior_lib/act/DealChat.py b/robowaiter/behavior_lib/act/DealChat.py index 04133d7..ea6c8da 100644 --- 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"] diff --git a/robowaiter/behavior_lib/act/ExploreEnv.py b/robowaiter/behavior_lib/act/ExploreEnv.py index 3b832b1..8179d2b 100644 --- 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 diff --git a/robowaiter/behavior_lib/act/GreatCustomer.py b/robowaiter/behavior_lib/act/GreatCustomer.py new file mode 100644 index 0000000..5ac8844 --- /dev/null +++ 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 diff --git a/robowaiter/behavior_lib/act/Make.py b/robowaiter/behavior_lib/act/Make.py index 88f828e..f3e9b38 100644 --- 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: diff --git a/robowaiter/behavior_lib/act/MoveTo.py b/robowaiter/behavior_lib/act/MoveTo.py index 6c31ad5..ac419f9 100644 --- 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: # 是否用容器装好 diff --git a/robowaiter/behavior_lib/act/PickUp.py b/robowaiter/behavior_lib/act/PickUp.py index 30aec3f..83b1fe0 100644 --- 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 diff --git a/robowaiter/behavior_lib/act/PutDown.py b/robowaiter/behavior_lib/act/PutDown.py index 7187a69..d9a5770 100644 --- 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 diff --git a/robowaiter/behavior_lib/cond/At.py b/robowaiter/behavior_lib/cond/At.py index 4797557..a217217 100644 --- 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) diff --git a/robowaiter/behavior_lib/cond/DetectCustomer.py b/robowaiter/behavior_lib/cond/DetectCustomer.py new file mode 100644 index 0000000..76d2232 --- /dev/null +++ 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 \ No newline at end of file diff --git a/robowaiter/behavior_lib/cond/On.py b/robowaiter/behavior_lib/cond/On.py index 0060e2e..b2a6040 100644 --- 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): diff --git a/robowaiter/llm_client/data/test_questions.txt b/robowaiter/llm_client/data/test_questions.txt index 6959b5f..a0ba46a 100644 --- 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)\"}"}} diff --git a/robowaiter/llm_client/data_raw/test_questions.csv b/robowaiter/llm_client/data_raw/test_questions.csv index 8a71f42..4ce218c 100644 --- 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)""}" diff --git a/robowaiter/llm_client/find_obj_utils.py b/robowaiter/llm_client/find_obj_utils.py new file mode 100644 index 0000000..2d940ae --- /dev/null +++ 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) \ No newline at end of file diff --git a/robowaiter/llm_client/tool_api.py b/robowaiter/llm_client/tool_api.py index 7ab10a6..51be613 100644 --- 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 \ No newline at end of file diff --git a/robowaiter/llm_client/tool_register.py b/robowaiter/llm_client/tool_register.py index 4360677..36f204b 100644 --- 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"})) diff --git a/robowaiter/robot/Default.ptml b/robowaiter/robot/Default.ptml index 62a677c..e6addee 100644 --- 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() + } } \ No newline at end of file diff --git a/robowaiter/robot/robot.py b/robowaiter/robot/robot.py index 85d135a..bc79121 100644 --- 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()") diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index 6aa2ec2..418e861 100644 --- 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) \ No newline at end of file + # 根据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 + + + + diff --git a/robowaiter/scene/tasks/Open_tasks_test.py b/robowaiter/scene/tasks/Open_tasks_test.py index 0408d60..87d1f02 100644 --- 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 diff --git a/robowaiter/scene/tasks/VLM/VLM.py b/robowaiter/scene/tasks/VLM/VLM.py index 5631c27..d0ba4a5 100644 --- 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]] diff --git a/robowaiter/scene/tasks/VLM/VLM2.py b/robowaiter/scene/tasks/VLM/VLM2.py index fa3595f..1f23690 100644 --- 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 diff --git a/robowaiter/scene/tasks/VLM/VLM3.py b/robowaiter/scene/tasks/VLM/VLM3.py new file mode 100644 index 0000000..f83416c --- /dev/null +++ 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()