diff --git a/robowaiter/behavior_lib/_base/Behavior.py b/robowaiter/behavior_lib/_base/Behavior.py index 8baf2ca..27e1ba6 100644 --- a/robowaiter/behavior_lib/_base/Behavior.py +++ b/robowaiter/behavior_lib/_base/Behavior.py @@ -83,6 +83,7 @@ class Bahavior(ptree.behaviour.Behaviour): # let behavior node interact with the scene def set_scene(self, scene): self.scene = scene + self.robot = scene.robot def setup(self, **kwargs: Any) -> None: return super().setup(**kwargs) diff --git a/robowaiter/behavior_lib/act/DealChat.py b/robowaiter/behavior_lib/act/DealChat.py index ea6c8da..bb69360 100644 --- a/robowaiter/behavior_lib/act/DealChat.py +++ b/robowaiter/behavior_lib/act/DealChat.py @@ -10,6 +10,9 @@ class DealChat(Act): def _update(self) -> ptree.common.Status: # if self.scene.status? + name,sentence = self.scene.state['chat_list'][0] + + chat = self.scene.state['chat_list'].pop() if isinstance(chat,set): self.create_sub_task(chat) diff --git a/robowaiter/behavior_lib/act/GreetCustomer.py b/robowaiter/behavior_lib/act/GreetCustomer.py new file mode 100644 index 0000000..9297dd4 --- /dev/null +++ b/robowaiter/behavior_lib/act/GreetCustomer.py @@ -0,0 +1,31 @@ +import py_trees as ptree +from robowaiter.behavior_lib._base.Act import Act +from robowaiter.algos.navigator.navigate import Navigator + +class GreetCustomer(Act): + can_be_expanded = True + 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("欢迎光临！请问有什么可以帮您？") + + customer_name = self.scene.state['attention']['customer'] + self.scene.state['greeted_customers'].add(customer_name) + + return ptree.common.Status.RUNNING diff --git a/robowaiter/behavior_lib/act/GreatCustomer.py b/robowaiter/behavior_lib/act/ServeCustomer.py similarity index 90% rename from robowaiter/behavior_lib/act/GreatCustomer.py rename to robowaiter/behavior_lib/act/ServeCustomer.py index 5ac8844..1b4312e 100644 --- a/robowaiter/behavior_lib/act/GreatCustomer.py +++ b/robowaiter/behavior_lib/act/ServeCustomer.py @@ -2,7 +2,7 @@ import py_trees as ptree from robowaiter.behavior_lib._base.Act import Act from robowaiter.algos.navigator.navigate import Navigator -class GreatCustomer(Act): +class ServeCustomer(Act): can_be_expanded = False num_args = 0 valid_args = () @@ -14,7 +14,7 @@ class GreatCustomer(Act): def get_info(cls): info = {} info['pre'] = set() - info["add"] = set() + info["add"] = {"CustomerServed()"} info["del_set"] = set() info['cost']=0 return info diff --git a/robowaiter/behavior_lib/cond/CustomerServed.py b/robowaiter/behavior_lib/cond/CustomerServed.py new file mode 100644 index 0000000..179660e --- /dev/null +++ b/robowaiter/behavior_lib/cond/CustomerServed.py @@ -0,0 +1,24 @@ +import py_trees as ptree +from typing import Any +from robowaiter.behavior_lib._base.Cond import Cond +import itertools + +class CustomerServed(Cond): + can_be_expanded = True + + def __init__(self,*args): + super().__init__(*args) + + + def _update(self) -> ptree.common.Status: + # if self.scene.status? + + if self.name in self.scene.state["condition_set"]: + return ptree.common.Status.SUCCESS + else: + return ptree.common.Status.FAILURE + + # if self.scene.state['chat_list'] == []: + # return ptree.common.Status.FAILURE + # else: + # return ptree.common.Status.SUCCESS diff --git a/robowaiter/behavior_lib/cond/DetectCustomer.py b/robowaiter/behavior_lib/cond/DetectCustomer.py index 76d2232..6ac8bf7 100644 --- a/robowaiter/behavior_lib/cond/DetectCustomer.py +++ b/robowaiter/behavior_lib/cond/DetectCustomer.py @@ -18,14 +18,21 @@ class DetectCustomer(Cond): # bar (247.0, 520.0, 100.0) close_to_bar = False scene = self.scene.status + queue_list = [] 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 + # close_to_bar = True + queue_list.append((x,y,walker.name)) - if close_to_bar: + if queue_list == []: + return ptree.common.Status.FAILURE + + queue_list.sort() + x,y,name = queue_list[0] + if name not in self.scene.state["greet_set"]: + self.scene.state['attention']["customer"] = name return ptree.common.Status.SUCCESS else: return ptree.common.Status.FAILURE \ No newline at end of file diff --git a/robowaiter/llm_client/tool_api_multi_round.py b/robowaiter/llm_client/tool_api_multi_round.py index 0df90d9..aa6e78f 100644 --- a/robowaiter/llm_client/tool_api_multi_round.py +++ b/robowaiter/llm_client/tool_api_multi_round.py @@ -36,7 +36,7 @@ if __name__ == "__main__": question = input("\n顾客：") data_memory = [{ "role": "system", - "content": "你是RoboWaiter,一个由HPCL团队开发的机器人服务员，你在咖啡厅工作。接受顾客的指令并调用工具函数来完成各种服务任务。", + "content": "你是RoboWaiter,一个由HPCL团队开发的机器人服务员，你在咖啡厅工作。接受顾客的指令并调用工具函数来完成各种服务任务。如果顾客问你们这里有什么，或者想要点单，你说我们咖啡厅提供咖啡，水，点心，酸奶等食物。如果顾客不需要你了，你就回到吧台招待。", },] n = 1 max_retry = 5 diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index 418e861..7088e8f 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -49,6 +49,7 @@ def show_image(camera_data): class Scene: robot = None event_list = [] + new_event_list = [] show_bubble = False default_state = { @@ -62,7 +63,12 @@ class Scene: "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)'} + 'Is(Table1,Dirty)', 'Is(Floor,Dirty)', 'Is(Chairs,Dirty)'}, + "obj_mem":{}, + "customer_mem":{}, + "served_mem":{}, + "greeted_customers":set(), + "attention":{} } """ status: @@ -143,9 +149,20 @@ class Scene: self.time = time.time() - self.start_time self.deal_event() + self.deal_new_event() self._step() self.robot.step() + def deal_new_event(self): + if len(self.new_event_list)>0: + next_event = self.new_event_list[0] + t,func,args = next_event + if self.time >= t: + print(f'event: {t}, {func.__name__}') + self.new_event_list.pop(0) + func(*args) + + def deal_event(self): if len(self.event_list)>0: next_event = self.event_list[0] @@ -171,6 +188,10 @@ class Scene: return set_sub_task + def new_set_goal(self,goal): + g = eval("{'" + goal + "'}") + self.state['chat_list'].append(g) + @property def status(self): @@ -196,7 +217,6 @@ class Scene: pass - 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] @@ -216,6 +236,10 @@ class Scene: return scene + def walker_walk_to(self,walkerID,X,Y,speed=50,Yaw=0): + self.control_walker( + [self.walker_control_generator(walkerID=walkerID, autowalk=False, speed=speed, X=X, Y=Y, Yaw=Yaw)]) + def reachable_check(self, X, Y, Yaw): if self.use_offset: @@ -243,9 +267,19 @@ class Scene: 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]))) + GrabSim_pb2.WalkerList.Walker(id=id, pose=GrabSim_pb2.Pose(X=loc[0], Y=loc[1], Yaw=loc[2]))) stub.AddWalker(GrabSim_pb2.WalkerList(walkers=walker_list, scene=self.sceneID)) + w = self.status.walkers + num_customer = len(w) + self.state["customer_mem"][w[-1].name] = num_customer-1 + + def walker_index2mem(self,index): + for mem,i in self.state["customer_mem"].items(): + if index == i: + return mem + + def add_walkers(self,walker_loc=[[0, 880], [250, 1200], [-55, 750], [70, -200]]): print('------------------add_walkers----------------------') for id,walker in enumerate(walker_loc): @@ -267,8 +301,16 @@ class Scene: # walkerID is the index of the walker in status.walkers. # Since status.walkers is a list, some walkerIDs would change after removing a walker. remove_list.append(walkerID) + + index_shift_list = [ 0 for _ in range(len(self.state["customer_mem"])) ] + stub.RemoveWalkers(GrabSim_pb2.RemoveList(IDs=remove_list, scene=self.sceneID)) + w = self.status.walkers + for i in range(len(w)): + self.state["customer_mem"][w[i].name] = i + + def clean_walker(self): stub.CleanWalkers(GrabSim_pb2.SceneID(value=self.sceneID)) @@ -382,11 +424,15 @@ class Scene: ) ) - # def walker_bubble(self, message): - # status = self.status - # walker_name = status.walkers[0].name - # talk_content = walker_name + ":" + message - # self.control_robot_action(0, 0, 3, talk_content) + def walker_bubble(self, name, message): + talk_content = name + ":" + message + self.control_robot_action(0, 3, talk_content) + + def customer_say(self,name,sentence,show_bubble=True): + print(f'{name} say: {sentence}') + if show_bubble: + self.walker_bubble(name,sentence) + self.state['chat_list'].append((name,sentence)) # def control_robot_action(self, scene_id=0, type=0, action=0, message="你好"): # print('------------------control_robot_action----------------------') @@ -474,6 +520,8 @@ class Scene: scene = stub.Do(action) print("After Walk Position:", [scene.location.X, scene.location.Y, scene.rotation.Yaw]) + + # 相应的行动，由主办方封装 def control_robot_action(self, type=0, action=0, message="你好"): scene = stub.ControlRobot( diff --git a/robowaiter/scene/tasks/Auto_tasks.py b/robowaiter/scene/tasks/Auto_tasks.py index 89b9e6b..f7f8135 100644 --- a/robowaiter/scene/tasks/Auto_tasks.py +++ b/robowaiter/scene/tasks/Auto_tasks.py @@ -32,7 +32,6 @@ class SceneAT(Scene): pass if __name__ == '__main__': - import os from robowaiter.robot.robot import Robot robot = Robot() diff --git a/robowaiter/scene/tasks/Open_tasks.py b/robowaiter/scene/tasks/OT/Open_tasks.py similarity index 100% rename from robowaiter/scene/tasks/Open_tasks.py rename to robowaiter/scene/tasks/OT/Open_tasks.py diff --git a/robowaiter/scene/tasks/Open_tasks_test.py b/robowaiter/scene/tasks/OT/Open_tasks_bright_table.py similarity index 100% rename from robowaiter/scene/tasks/Open_tasks_test.py rename to robowaiter/scene/tasks/OT/Open_tasks_bright_table.py diff --git a/robowaiter/scene/tasks/OT/Open_tasks_test.py b/robowaiter/scene/tasks/OT/Open_tasks_test.py new file mode 100644 index 0000000..95408bf --- /dev/null +++ b/robowaiter/scene/tasks/OT/Open_tasks_test.py @@ -0,0 +1,57 @@ +""" +人提出请求，机器人完成任务 +1. 做咖啡（固定动画）：接收到做咖啡指令、走到咖啡机、拿杯子、操作咖啡机、取杯子、送到客人桌子上 +2. 倒水 +3. 夹点心 + +具体描述：设计一套点单规则（如菜单包含咖啡、水、点心等），按照规则拟造随机的订单。在收到订单后，通过大模型让机器人输出合理的备餐计划，并尝试在模拟环境中按照这个规划实现任务。 + +""" + +# todo: 接收点单信息，大模型生成任务规划 + +from robowaiter.scene.scene import Scene + +class SceneOT(Scene): + + def __init__(self, robot): + super().__init__(robot) + # 在这里加入场景中发生的事件 + self.new_event_list = [ + # (9,self.add_walkers,([[0, 880]],)), + # (10,self.walker_walk_to,(2,50,500)) + # (5, self.set_goal("On(Yogurt,Table4)")) + # (5, self.set_goal("At(Robot,BrightTable4)")) + ] + + def _reset(self): + # self.add_walkers([[0, 880], [250, 1200]]) + + # 展示顾客，前8个id是小孩，后面都是大人 + for i in range(4): + self.add_walker(i,50,300 + i * 50) + name1 = self.walker_index2mem(1) + name2 = self.walker_index2mem(3) + + self.remove_walker(0,2) + + index1 = self.state["customer_mem"][name1] + index2 = self.state["customer_mem"][name2] + + self.walker_bubble(name1,f"我是第{index1}个") + self.walker_bubble(name2,f"我是第{index2}个") + + def _run(self): + pass + + +if __name__ == '__main__': + import os + from robowaiter.robot.robot import Robot + + robot = Robot() + + # create task + task = SceneOT(robot) + task.reset() + task.run() diff --git a/robowaiter/scene/tasks/OT/__init__.py b/robowaiter/scene/tasks/OT/__init__.py new file mode 100644 index 0000000..e69de29