diff --git a/robowaiter/behavior_lib/_base/Act.py b/robowaiter/behavior_lib/_base/Act.py index d2e95e5..d2b14fb 100644 --- a/robowaiter/behavior_lib/_base/Act.py +++ b/robowaiter/behavior_lib/_base/Act.py @@ -3,10 +3,18 @@ from robowaiter.behavior_lib._base.Behavior import Bahavior class Act(Bahavior): print_name_prefix = "act " type = 'Act' - all_place = {'Bar', 'WaterTable', 'CoffeeTable', 'Bar2', 'Table1', 'Table2', 'Table3'} + all_place = {'Bar', 'Bar2', 'WaterTable', 'CoffeeTable', 'Table1', 'Table2', 'Table3'} all_object = {'Coffee', 'Water', 'Dessert', 'Softdrink', 'BottledDrink', 'Yogurt', 'ADMilk', 'MilkDrink', 'Milk', 'VacuumCup'} - + place_xyz_dic={ + 'Bar': (247.0, 520.0, 100.0), + 'Bar2': (240.0, 40.0, 70.0), + 'WaterTable':(-70.0, 500.0, 107), + 'CoffeeTable':(247.0, 520.0, 100.0), # 位置需要更改!!! + 'Table1': (247.0, 520.0, 100.0),# 位置需要更改!!! + 'Table2': (-55.0, 0.0, 107), + 'Table3':(-55.0, 150.0, 107) + } def __init__(self,*args): super().__init__(*args) self.info = self.get_info(*args) diff --git a/robowaiter/behavior_lib/act/Clean.py b/robowaiter/behavior_lib/act/Clean.py new file mode 100644 index 0000000..81c7af1 --- /dev/null +++ b/robowaiter/behavior_lib/act/Clean.py @@ -0,0 +1,47 @@ +import py_trees as ptree +from typing import Any +from robowaiter.behavior_lib._base.Act import Act +from robowaiter.behavior_lib._base.Behavior import Status + +class Clean(Act): + can_be_expanded = True + num_args = 1 + valid_args = ( + 'Table1','Floor','Chairs' + ) + + def __init__(self, *args): + super().__init__(*args) + self.target_obj = self.args[0] + self.op_type = 5 + if self.target_obj=="Table1": + self.op_type = 5 + elif self.target_obj=="Floor": + self.op_type = 4 + elif self.target_obj=="Chairs": + self.op_type = 7 + + + @classmethod + def get_info(cls,arg): + info = {} + info["pre"]= {f'Holding(Nothing)'} + if arg == "Table1": + info["add"]= {f'Is(Table1,Clean)'} + info["del"] = {f'Is(Table1,Dirty)'} + elif arg == "Floor": + info["add"] = {f'Is(Floor,Clean)'} + info["del"] = {f'Is(Floor,Dirty)'} + elif arg == "Chairs": + info["add"] = {f'Is(Chairs,Clean)'} + info["del"] = {f'Is(Chairs,Dirty)'} + return info + + def _update(self) -> ptree.common.Status: + + self.scene.move_task_area(self.op_type) + self.scene.op_task_execute(self.op_type) + + self.scene.state["condition_set"].union(self.info["add"]) + self.scene.state["condition_set"] -= self.info["del"] + return Status.RUNNING \ No newline at end of file diff --git a/robowaiter/behavior_lib/act/Make.py b/robowaiter/behavior_lib/act/Make.py index ade04b1..ef97eb9 100644 --- a/robowaiter/behavior_lib/act/Make.py +++ b/robowaiter/behavior_lib/act/Make.py @@ -13,6 +13,13 @@ class Make(Act): def __init__(self, *args): super().__init__(*args) self.target_obj = self.args[0] + self.op_type = 1 + if self.target_obj=="Coffee": + self.op_type = 1 + elif self.target_obj=="Water": + self.op_type = 2 + elif self.target_obj=="Dessert": + self.op_type = 3 @classmethod @@ -29,9 +36,10 @@ class Make(Act): return info def _update(self) -> ptree.common.Status: - op_type = 1 - self.scene.move_task_area(op_type) - self.scene.op_task_execute(op_type) + + self.scene.move_task_area(self.op_type) + self.scene.op_task_execute(self.op_type) + self.scene.state["condition_set"].union(self.info["add"]) self.scene.state["condition_set"] -= self.info["del"] return Status.RUNNING \ No newline at end of file diff --git a/robowaiter/behavior_lib/act/PickUp.py b/robowaiter/behavior_lib/act/PickUp.py new file mode 100644 index 0000000..2e3f0f1 --- /dev/null +++ b/robowaiter/behavior_lib/act/PickUp.py @@ -0,0 +1,34 @@ +import py_trees as ptree +from typing import Any +from robowaiter.behavior_lib._base.Act import Act +from robowaiter.behavior_lib._base.Behavior import Status + +class PickUp(Act): + can_be_expanded = True + num_args = 1 + + def __init__(self, *args): + super().__init__(*args) + self.target_obj = self.args[0] + + + @classmethod + def get_info(self,arg): + info = {} + info["pre"] = {f'At(Robot,{arg})','Holding(Nothing)'} + info["add"] = {f'Holding({arg})'} + info["del"] = {f'Holding(Nothing)'} + return info + + + def _update(self) -> ptree.common.Status: + # self.scene.test_move() + op_type=16 + obj_id = 0 + # 遍历场景里的所有物品,根据名字匹配位置最近的 obj-id + + self.scene.op_task_execute(op_type, obj_id=obj_id) + + self.scene.state["condition_set"].union(self.info["add"]) + self.scene.state["condition_set"] -= self.info["del"] + return Status.RUNNING diff --git a/robowaiter/behavior_lib/act/PutDown.py b/robowaiter/behavior_lib/act/PutDown.py new file mode 100644 index 0000000..0fe363c --- /dev/null +++ b/robowaiter/behavior_lib/act/PutDown.py @@ -0,0 +1,35 @@ +import py_trees as ptree +from typing import Any +from robowaiter.behavior_lib._base.Act import Act +from robowaiter.behavior_lib._base.Behavior import Status + +class PutDown(Act): + can_be_expanded = True + num_args = 1 + + def __init__(self, *args): + super().__init__(*args) + self.target_obj = self.args[0] + self.target_place = self.args[1] + + + @classmethod + def get_info(self,arg): + info = {} + info["pre"] = {f'Holding({arg[0]})',f'At(Robot,{arg[1]})'} + info["add"] = {f'Holding(Nothing)',f'At({arg[0]},{arg[1]})'} + info["del"] = {f'Holding(Nothing)'} + return info + + + def _update(self) -> ptree.common.Status: + # self.scene.test_move() + op_type=17 + release_pos = list(Act.place_xyz_dic[self.target_place]) + # # 原始吧台处:[247.0, 520.0, 100.0], 空调开关旁吧台:[240.0, 40.0, 70.0], 水杯桌:[-70.0, 500.0, 107] + # # 桌子2:[-55.0, 0.0, 107],桌子3:[-55.0, 150.0, 107] + self.scene.op_task_execute(op_type, release_pos=release_pos) + + self.scene.state["condition_set"].union(self.info["add"]) + self.scene.state["condition_set"] -= self.info["del"] + return Status.RUNNING diff --git a/robowaiter/behavior_lib/act/Turn.py b/robowaiter/behavior_lib/act/Turn.py new file mode 100644 index 0000000..ed47bde --- /dev/null +++ b/robowaiter/behavior_lib/act/Turn.py @@ -0,0 +1,64 @@ +import py_trees as ptree +from typing import Any +from robowaiter.behavior_lib._base.Act import Act +from robowaiter.behavior_lib._base.Behavior import Status + +class Clean(Act): + can_be_expanded = True + num_args = 1 + valid_args = [('AC','ACTemperature','TubeLight','HallLight','Curtain'), + ('Off','On','Up','Down','Clean','Dirty')] + + def __init__(self, *args): + super().__init__(*args) + self.target_obj = self.args[0] + self.op = self.args[1] + self.op_type = 13 + + if self.target_obj=="AC": + self.op_type = 13 + elif self.target_obj=="ACTemperature": + if self.op == 'Up': + self.op_type = 14 + elif self.op == 'Down': + self.op_type = 15 + elif self.target_obj=="TubeLight": + if self.op == 'On': + self.op_type = 6 + elif self.op == 'Off': + self.op_type = 8 + elif self.target_obj=="HallLight": + if self.op == 'On': + self.op_type = 9 + elif self.op == 'Off': + self.op_type = 10 + elif self.target_obj=="Curtain": + if self.op == 'On': + self.op_type = 11 + elif self.op == 'Off': + self.op_type = 12 + + @classmethod + def get_info(cls,arg): + info = {} + # 明天写 + # info["pre"]= {f'Holding(Nothing)'} + # if arg == "Table1": + # info["add"]= {f'Is(Table1,Clean)'} + # info["del"] = {f'Is(Table1,Dirty)'} + # elif arg == "Floor": + # info["add"] = {f'Is(Floor,Clean)'} + # info["del"] = {f'Is(Floor,Dirty)'} + # elif arg == "Chairs": + # info["add"] = {f'Is(Chairs,Clean)'} + # info["del"] = {f'Is(Chairs,Dirty)'} + return info + + def _update(self) -> ptree.common.Status: + + self.scene.move_task_area(self.op_type) + self.scene.op_task_execute(self.op_type) + + self.scene.state["condition_set"].union(self.info["add"]) + self.scene.state["condition_set"] -= self.info["del"] + return Status.RUNNING \ No newline at end of file