EmbodiedAI
/
RoboWaiter
mirror of https://github.com/HPCL-EI/RoboWaiter.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'main' of https://github.com/HPCL-EI/RoboWaiter 

# Conflicts:
#	robowaiter/scene/scene.py
This commit is contained in:
Netceor 2023-11-17 13:12:05 +08:00
parent 30eb82f7a8 b899599b04
commit 5ccd53b02f
11 changed files with 95 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
robowaiter/behavior_lib/_base/Behavior.py
View File

@ -29,8 +29,7 @@ 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)
}

5
robowaiter/behavior_lib/act/ExploreEnv.py
View File

@ -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

27
robowaiter/behavior_lib/act/GreatCustomer.py Normal file
View File

@ -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

1
robowaiter/behavior_lib/act/Make.py
View File

@ -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:

2
robowaiter/behavior_lib/act/MoveTo.py
View File

@ -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

2
robowaiter/behavior_lib/act/PutDown.py
View File

@ -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

31
robowaiter/behavior_lib/cond/DetectCustomer.py Normal file
View File

@ -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

11
robowaiter/robot/Default.ptml
View File

@ -14,5 +14,14 @@ selector
act SubTaskPlaceHolder()
}
}
sequence
{
selector
{
cond At(Robot,Bar)
act MoveTo(Bar)
}
cond DetectCustomer()
act GreatCustomer
}
}

1
robowaiter/robot/robot.py
View File

@ -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()")

20
robowaiter/scene/scene.py
View File

@ -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",
@ -110,6 +112,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):
@ -176,8 +181,8 @@ class Scene:
def reset_sim(self):
# reset world
init_world()
stub.Reset(GrabSim_pb2.ResetParams(scene=self.sceneID))
stub.CleanWalkers(GrabSim_pb2.SceneID(value=self.sceneID))
def _reset(self):
# 场景自定义的reset
@ -709,4 +714,13 @@ 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)
# 根据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

60
robowaiter/scene/tasks/VLM/VLM2.py
View File

@ -11,28 +11,11 @@ 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号桌")),
]
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()
pass
def _run(self, op_type=10):
@ -42,17 +25,13 @@ class SceneVLM(Scene):
# 带领行人去有太阳的地方
# 行人说 有点热
# 好的，这就去开空调
scene = self.add_walkers([[47, 920],[70,-200]])
scene = self.add_walkers([[47, 920]])
self.control_walker(
[self.walker_control_generator(walkerID=0, autowalk=False, speed=200, X=47, Y=520, Yaw=0)])
[self.walker_control_generator(walkerID=0, autowalk=False, speed=200, X=60, Y=520, Yaw=0)])
cont = scene.walkers[0].name+":请问可以带我去空位上嘛？我想晒太阳"
self.control_robot_action(0,3,cont)
# self.clean_walker()
# 共17个操作
# "制作咖啡","倒水","夹点心","拖地","擦桌子","开筒灯","搬椅子", # 1-7
# "关筒灯","开大厅灯","关大厅灯","关闭窗帘","打开窗帘", # 8-12
@ -73,39 +52,6 @@ class SceneVLM(Scene):
# 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