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

在操作任务中增加了 视觉图像分割

This commit is contained in:
Caiyishuai 2023-11-20 11:12:36 +08:00
parent 66ce119e12
commit 20706a28d1
18 changed files with 101 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -44,6 +44,7 @@ class Clean(Act):
self.scene.state["condition_set"] |= (self.info["add"])
self.scene.state["condition_set"] -= self.info["del_set"]
self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
# print("After Clean condition_set:",self.scene.state["condition_set"] )
return Status.RUNNING

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

@ -54,6 +54,8 @@ class Make(Act):
# obj_info = obj_dict[id]
# obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z
# print(id,obj.name,obj_x,obj_y,obj_z)
if self.scene.take_picture:
self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
self.scene.state["condition_set"] |= (self.info["add"])
self.scene.state["condition_set"] -= self.info["del_set"]

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

@ -32,6 +32,9 @@ class MoveTo(Act):
# goal = self.scene.state['map']['obj_pos'][self.args[0]]
# navigator.navigate_old(goal, animation=False)
if self.scene.take_picture:
self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
# 走到固定的地点
if self.target_place in Act.place_xy_yaw_dic:
goal = Act.place_xy_yaw_dic[self.target_place]
@ -87,10 +90,12 @@ class MoveTo(Act):
# print("MoveTo",obj_x, obj_y, obj_z," obj_id:",obj_id," obj_info:",obj_info.name)
if self.scene.take_picture:
self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
# goal = self.scene.state['map']['obj_pos'][self.args[0]]
# self.scene.walk_to(goal[0],goal[1]) # X, Y, Yaw=None, velocity=200, dis_limit=0
self.scene.state["condition_set"] |= (self.info["add"])
self.scene.state["condition_set"] -= self.info["del_set"]
return ptree.common.Status.RUNNING

3
robowaiter/behavior_lib/act/PickUp.py
View File

@ -56,6 +56,9 @@ class PickUp(Act):
self.scene.move_task_area(op_type=16, obj_id=obj_id)
self.scene.op_task_execute(op_type=16, obj_id=obj_id)
if self.scene.take_picture:
self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
self.scene.state["condition_set"] |= (self.info["add"])
self.scene.state["condition_set"] -= self.info["del_set"]
return Status.RUNNING

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

@ -41,6 +41,8 @@ class PutDown(Act):
self.scene.move_task_area(op_type, release_pos=release_pos)
self.scene.op_task_execute(op_type, release_pos=release_pos)
if self.scene.take_picture:
self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
self.scene.state["condition_set"] |= (self.info["add"])
self.scene.state["condition_set"] -= self.info["del_set"]

3
robowaiter/behavior_lib/act/Turn.py
View File

@ -76,7 +76,8 @@ class Turn(Act):
self.scene.move_task_area(self.op_type)
self.scene.op_task_execute(self.op_type)
if self.scene.take_picture:
self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
self.scene.state["condition_set"] |= (self.info["add"])
self.scene.state["condition_set"] -= self.info["del_set"]

2
robowaiter/behavior_lib/cond/At.py
View File

@ -18,6 +18,8 @@ class At(Cond):
def _update(self) -> ptree.common.Status:
# if self.scene.status?
# self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
if self.name in self.scene.state["condition_set"]:
return ptree.common.Status.SUCCESS
else:

2
robowaiter/behavior_lib/cond/FocusingCustomer.py
View File

@ -11,6 +11,8 @@ class FocusingCustomer(Cond):
def _update(self) -> ptree.common.Status:
# if self.scene.status?
if "customer" in self.scene.state['attention']:
if self.scene.take_picture:
self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
return ptree.common.Status.SUCCESS
else:
goal = Cond.place_xy_yaw_dic['Bar']

2
robowaiter/behavior_lib/cond/Holding.py
View File

@ -14,6 +14,8 @@ class Holding(Cond):
def _update(self) -> ptree.common.Status:
# if self.scene.status?
# self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
if self.name in self.scene.state["condition_set"]:
return ptree.common.Status.SUCCESS
else:

1
robowaiter/behavior_lib/cond/Is.py
View File

@ -24,6 +24,7 @@ class Is(Cond):
def _update(self) -> ptree.common.Status:
# if self.scene.status?
# self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
if self.name in self.scene.state["condition_set"]:
return ptree.common.Status.SUCCESS

2
robowaiter/behavior_lib/cond/NewCustomerComing.py
View File

@ -14,6 +14,8 @@ class NewCustomerComing(Cond):
def _update(self) -> ptree.common.Status:
if self.scene.take_picture:
self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
# 获取customer的位置
# bar (247.0, 520.0, 100.0)
close_to_bar = False

1
robowaiter/behavior_lib/cond/On.py
View File

@ -18,6 +18,7 @@ class On(Cond):
# print("self.name:",self.name)
# print("On: condition_set:",self.scene.state["condition_set"])
# self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
if self.name in self.scene.state["condition_set"]:
return ptree.common.Status.SUCCESS

5
robowaiter/robot/robot.py
View File

@ -92,8 +92,9 @@ class Robot(object):
else:
print('Warning: have none sub task sequence')
self.scene.sub_task_seq = seq
print("当前行为树为：")
print_tree_from_root(self.bt.root)
# print("当前行为树为：")
# print_tree_from_root(self.bt.root)
print("行为树扩展完成！")
# 获取所有action的pre,add,del列表
def collect_action_nodes(self):

69
robowaiter/scene/scene.py
View File

@ -15,6 +15,12 @@ from robowaiter.proto import GrabSim_pb2_grpc
import copy
import os
from robowaiter.utils import get_root_path
from sklearn.cluster import DBSCAN
from matplotlib import pyplot as plt
plt.rcParams['font.sans-serif']=['SimHei'] #用来正常显示中文标签
plt.rcParams['axes.unicode_minus']=False #用来正常显示负号
root_path = get_root_path()
channel = grpc.insecure_channel(
@ -35,7 +41,7 @@ def init_world(scene_num=1, mapID=11):
time.sleep(3) # wait for the map to load
def get_camera(part, scene_id=0):
print('------------------get_camera----------------------')
# print('------------------get_camera----------------------')
action = GrabSim_pb2.CameraList(cameras=part, scene=scene_id)
return stub.Capture(action)
@ -107,6 +113,10 @@ class Scene:
self.sub_task_seq = None
self.show_bubble = True
# 图像分割
self.take_picture = False
self.map_ratio = 5
self.db = DBSCAN(eps=self.map_ratio, min_samples=int(self.map_ratio / 2))
# init robot
if robot:
@ -576,7 +586,17 @@ class Scene:
def move_to_obj(self,obj_id):
scene = self.status
# 抬头
# value = [0]*21
# for i in range(21):
# value[i] = self.status.joints[i].angle
# value[5] = 0
# action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
# scene = stub.Do(action)
# time.sleep(1.0)
obj_info = scene.objects[obj_id]
# Robot
obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z
@ -595,6 +615,16 @@ class Scene:
# 移动到进行操作任务的指定地点
def move_task_area(self, op_type, obj_id=0, release_pos=[247.0, 520.0, 100.0]):
scene = self.status
# 抬头
# value = [0]*21
# for i in range(21):
# value[i] = self.status.joints[i].angle
# value[5] = 0
# action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
# scene = stub.Do(action)
# time.sleep(1.0)
cur_pos = [scene.location.X, scene.location.Y, scene.rotation.Yaw]
print("Current Position:", cur_pos, "开始任务:", self.op_dialog[op_type])
if op_type == 11 or op_type == 12: # 开关窗帘不需要移动
@ -642,6 +672,15 @@ class Scene:
# 调整空调开关、温度
def adjust_kongtiao(self,op_type):
# 低头
value = [0]*21
for i in range(21):
value[i] = self.status.joints[i].angle
value[5] = 30
action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
scene = stub.Do(action)
time.sleep(1.0)
obj_loc = self.obj_loc[:]
obj_loc[2] -= 5
if op_type == 13: obj_loc[1] -= 2
@ -672,6 +711,19 @@ class Scene:
def grasp_obj(self,obj_id,hand_id=1):
print('------------------adjust_joints----------------------')
scene = self.status
# 低头
value = [0]*21
for i in range(21):
value[i] = self.status.joints[i].angle
value[5] = 30
action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
scene = stub.Do(action)
time.sleep(1.0)
if self.take_picture:
self.get_obstacle_point(self.db, self.status, map_ratio=self.map_ratio)
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":
@ -721,6 +773,19 @@ class Scene:
# 实现放置操作
def release_obj(self,release_pos):
print("------------------adjust_joints----------------------")
# 低头
value = [0]*21
for i in range(21):
value[i] = self.status.joints[i].angle
value[5] = 30
action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
scene = stub.Do(action)
time.sleep(1.0)
if self.take_picture:
self.get_obstacle_point(self.db, self.status, map_ratio=self.map_ratio)
if release_pos==[340.0, 900.0, 99.0]:
self.ik_control_joints(2, release_pos[0]-40, release_pos[1]+35, release_pos[2])
time.sleep(2.0)
@ -1093,6 +1158,8 @@ class Scene:
for key, value in object_pixels.items():
if key == 0:
continue
if key not in objs_id.keys():
continue
if key in [91, 84, 96, 87, 102, 106, 120, 85, 113, 101, 83, 251]:
X = np.array(value)
db.fit(X)

1
robowaiter/scene/tasks/CafeDailyOperations/VLN_greet_and_order.py
View File

@ -42,7 +42,6 @@ class SceneVLM(Scene):
(8, self.control_walkers_and_say, ([[[1, False, 100, -18, -200, -90, "你们这有什么饮料嘛？"]]])), # 20 胖胖男到了 BrightTable6
(2, self.customer_say, (1, "咖啡有哪些呢？")),# 10
(2, self.customer_say, (1,"来杯卡布奇诺吧。")), # 15
(2, self.customer_say, (1, "来杯卡布奇诺吧。")),
# # 场景3：有位女士要杯水和冰红茶

2
robowaiter/scene/tasks/VLM/VLM_1_order.py
View File

@ -13,7 +13,7 @@ class SceneVLM(Scene):
self.signal_event_list = [
(3, self.add_walker, (20,0,700)),
(1, self.control_walker, (6, False,100, 60, 520,0)),
(3, self.customer_say, (6, "给我来杯酸奶和冰红茶，我坐在对面的桌子那儿。")),
(1, self.customer_say, (6, "给我来杯酸奶和冰红茶，我坐在对面的桌子那儿。")),
(5, self.control_walker, (6, False, 100, -250, 480, 0)),
]

4
robowaiter/scene/tasks/VLM/VLM_2_AC.py
View File

@ -13,7 +13,7 @@ class SceneVLM(Scene):
self.signal_event_list = [
(3, self.add_walker, (0,0,700)),
(1, self.control_walker, (6, False,100, 60, 520,0)), #[walkerID,autowalk,speed,X,Y,Yaw]
(3, self.customer_say, (6, "好热呀，想开空调，想要温度调低点！")),
(1, self.customer_say, (6, "好热呀，想开空调，想要温度调低点！")),
(5, self.control_walker, (6, False, 200, 60, 80, 0)),
(-1, self.customer_say, (6, "谢谢！这下凉快了！")), #(-100,600)
]
@ -21,7 +21,7 @@ class SceneVLM(Scene):
def _reset(self):
self.gen_obj()
self.add_walkers([[4,1, 880], [31,250, 1200],[6,-55, 750],[10,70, -200],[27,-290, 400, 180],[26, 60,-320,90]])
self.control_walkers(walker_loc=[[-55, 750], [70, -200], [250, 1200], [0, 880]],is_autowalk = True)
self.control_walkers(walker_loc=[[-55, 750], [70, -200]],is_autowalk = True)
pass
def _run(self, op_type=10):

1
robowaiter/utils/bt/load.py
View File

@ -13,6 +13,7 @@ def load_bt_from_ptml(scene, ptml_path, behavior_lib_path):
print(f'BT loaded:')
print_tree_from_root(bt.root)
# print("行为树子树加载完毕")
# print(ptree.display.unicode_tree(root=bt.root, show_status=True))
return bt