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

sceneui

This commit is contained in:
ChenXL97 2023-11-23 10:03:47 +08:00
parent ae5dc2222e
commit 73f83cc56d
12 changed files with 747 additions and 397 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

254
robowaiter/scene/scene.py
View File

@ -1,3 +1,4 @@
import io
import pickle import pickle
import sys import sys
import time import time
@ -33,22 +34,10 @@ channel = grpc.insecure_channel(
("grpc.max_receive_message_length", 1024 * 1024 * 1024), ("grpc.max_receive_message_length", 1024 * 1024 * 1024),
], ],
) )
stub = GrabSim_pb2_grpc.GrabSimStub(channel)
animation_step = [4, 5, 7, 3, 3] animation_step = [4, 5, 7, 3, 3]
loc_offset = [-700, -1400] loc_offset = [-700, -1400]
def init_world(scene_num=1, mapID=11):
stub.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=mapID))
time.sleep(3) # wait for the map to load
def get_camera(part, scene_id=0):
# print('------------------get_camera----------------------')
action = GrabSim_pb2.CameraList(cameras=part, scene=scene_id)
return stub.Capture(action)
def show_image(camera_data): def show_image(camera_data):
print('------------------show_image----------------------') print('------------------show_image----------------------')
@ -114,6 +103,11 @@ class Scene:
""" """
def __init__(self, robot=None, sceneID=0): def __init__(self, robot=None, sceneID=0):
self.stub = GrabSim_pb2_grpc.GrabSimStub(channel)
self.robot = robot
self.sceneID = sceneID self.sceneID = sceneID
self.use_offset = False self.use_offset = False
self.start_time = time.time() self.start_time = time.time()
@ -137,13 +131,19 @@ class Scene:
self.map_map_real = pickle.load(file) self.map_map_real = pickle.load(file)
self.init_robot(robot)
self.robot_changed = False self.robot_changed = False
self.last_event_time = 0 self.last_event_time = 0
self.last_camera_time = -99999 self.last_camera_time = -99999
self.last_step_time = -99999 self.last_step_time = -99999
self.img_cache = {
"img_label_canvas":None,
"img_label_seg":None,
"img_label_obj":None,
"img_label_map":None,
}
# 1-7 正常执行, 8-10 控灯操作移动到6, 11-12窗帘操作不需要移动, # 1-7 正常执行, 8-10 控灯操作移动到6, 11-12窗帘操作不需要移动,
self.op_dialog = ["", "制作咖啡", "倒水", "夹点心", "拖地", "擦桌子", "开筒灯", "搬椅子", # 1-7 self.op_dialog = ["", "制作咖啡", "倒水", "夹点心", "拖地", "擦桌子", "开筒灯", "搬椅子", # 1-7
"关筒灯", "开大厅灯", "关大厅灯", "关闭窗帘", "打开窗帘", # 8-12 "关筒灯", "开大厅灯", "关大厅灯", "关闭窗帘", "打开窗帘", # 8-12
@ -218,13 +218,22 @@ class Scene:
self.init_algos() # 初始化各种算法类 self.init_algos() # 初始化各种算法类
def init_robot(self, robot):
# init robot
self.robot = robot
if robot: def init_world(self,scene_num=1, mapID=11):
robot.set_scene(self) self.stub.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=mapID))
return robot.load_BT() time.sleep(3) # wait for the map to load
def get_camera(self,part, scene_id=0):
# print('------------------get_camera----------------------')
action = GrabSim_pb2.CameraList(cameras=part, scene=scene_id)
return self.stub.Capture(action)
def init_robot(self):
# init robot
if self.robot:
self.robot.set_scene(self)
self.robot.load_BT()
def init_algos(self): def init_algos(self):
''' '''
@ -243,18 +252,19 @@ class Scene:
def reset(self): def reset(self):
# 基类reset，默认执行仿真器初始化操作 # 基类reset，默认执行仿真器初始化操作
self.reset_sim() self.reset_sim()
self.init_robot()
self.init_algos()
# reset state # reset state
self.state = copy.deepcopy(self.default_state) self.state = copy.deepcopy(self.default_state)
print("场景初始化完成")
self._reset() self._reset()
print("场景初始化完成")
self.running = True self.running = True
def run(self): def run(self):
# 基类run # 基类run
self._run() self._run()
# 运行并由robot打印每步信息 # 运行并由robot打印每步信息
while True: while True:
@ -329,13 +339,13 @@ class Scene:
@property @property
def status(self): def status(self):
return stub.Observe(GrabSim_pb2.SceneID(value=self.sceneID)) return self.stub.Observe(GrabSim_pb2.SceneID(value=self.sceneID))
def reset_sim(self): def reset_sim(self):
# reset world # reset world
stub.CleanWalkers(GrabSim_pb2.SceneID(value=self.sceneID)) self.stub.CleanWalkers(GrabSim_pb2.SceneID(value=self.sceneID))
init_world() self.init_world()
stub.Reset(GrabSim_pb2.ResetParams(scene=self.sceneID)) self.stub.Reset(GrabSim_pb2.ResetParams(scene=self.sceneID))
def _reset(self): def _reset(self):
# 场景自定义的reset # 场景自定义的reset
@ -364,7 +374,7 @@ class Scene:
action = GrabSim_pb2.Action( action = GrabSim_pb2.Action(
scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v
) )
scene = stub.Do(action) scene = self.stub.Do(action)
return scene return scene
@ -375,7 +385,7 @@ class Scene:
def reachable_check(self, X, Y, Yaw): def reachable_check(self, X, Y, Yaw):
if self.use_offset: if self.use_offset:
X, Y = X + loc_offset[0], Y + loc_offset[1] X, Y = X + loc_offset[0], Y + loc_offset[1]
navigation_info = stub.Do( navigation_info = self.stub.Do(
GrabSim_pb2.Action( GrabSim_pb2.Action(
scene=self.sceneID, scene=self.sceneID,
action=GrabSim_pb2.Action.ActionType.WalkTo, action=GrabSim_pb2.Action.ActionType.WalkTo,
@ -390,7 +400,7 @@ class Scene:
def add_walker(self, id, x, y, yaw=0, v=0, scope=100): def add_walker(self, id, x, y, yaw=0, v=0, scope=100):
loc = [x, y, yaw, v, scope] loc = [x, y, yaw, v, scope]
action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=loc) action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=loc)
scene = stub.Do(action) scene = self.stub.Do(action)
# print(scene.info) # print(scene.info)
walker_list = [] walker_list = []
if (str(scene.info).find('unreachable') > -1): if (str(scene.info).find('unreachable') > -1):
@ -398,7 +408,7 @@ class Scene:
else: else:
walker_list.append( walker_list.append(
GrabSim_pb2.WalkerList.Walker(id=id, 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)) self.stub.AddWalker(GrabSim_pb2.WalkerList(walkers=walker_list, scene=self.sceneID))
w = self.status.walkers w = self.status.walkers
num_customer = len(w) num_customer = len(w)
@ -431,15 +441,15 @@ class Scene:
# Since status.walkers is a list, some walkerIDs would change after removing a walker. # Since status.walkers is a list, some walkerIDs would change after removing a walker.
remove_list.append(walkerID) remove_list.append(walkerID)
stub.RemoveWalkers(GrabSim_pb2.RemoveList(IDs=remove_list, scene=self.sceneID)) self.stub.RemoveWalkers(GrabSim_pb2.RemoveList(IDs=remove_list, scene=self.sceneID))
self.state["customer_mem"] = {} self.state["customer_mem"] = {}
w = self.status.walkers w = self.status.walkers
for i in range(len(w)): for i in range(len(w)):
self.state["customer_mem"][w[i].name] = i self.state["customer_mem"][w[i].name] = i
def remove_walkers(self, IDs=[0]): def remove_walkers(self, IDs=[0]):
s = stub.Observe(GrabSim_pb2.SceneID(value=self.sceneID)) s = self.stub.Observe(GrabSim_pb2.SceneID(value=self.sceneID))
scene = stub.RemoveWalkers(GrabSim_pb2.RemoveList(IDs=IDs, scene=self.sceneID)) scene = self.stub.RemoveWalkers(GrabSim_pb2.RemoveList(IDs=IDs, scene=self.sceneID))
time.sleep(2) time.sleep(2)
self.state["customer_mem"] = {} self.state["customer_mem"] = {}
w = self.status.walkers w = self.status.walkers
@ -448,7 +458,7 @@ class Scene:
return return
def clean_walkers(self): def clean_walkers(self):
scene = stub.CleanWalkers(GrabSim_pb2.SceneID(value=self.sceneID)) scene = self.stub.CleanWalkers(GrabSim_pb2.SceneID(value=self.sceneID))
self.state["customer_mem"] = {} self.state["customer_mem"] = {}
return scene return scene
@ -458,13 +468,13 @@ class Scene:
walkerID = self.walker_index2mem(walkerID) walkerID = self.walker_index2mem(walkerID)
pose = GrabSim_pb2.Pose(X=X, Y=Y, Yaw=Yaw) pose = GrabSim_pb2.Pose(X=X, Y=Y, Yaw=Yaw)
scene = stub.ControlWalkers( scene = self.stub.ControlWalkers(
GrabSim_pb2.WalkerControls( GrabSim_pb2.WalkerControls(
controls=[GrabSim_pb2.WalkerControls.WControl(id=walkerID, autowalk=autowalk, speed=speed, pose=pose)], controls=[GrabSim_pb2.WalkerControls.WControl(id=walkerID, autowalk=autowalk, speed=speed, pose=pose)],
scene=self.sceneID) scene=self.sceneID)
) )
return scene return scene
# stub.ControlWalkers( # self.stub.ControlWalkers(
# GrabSim_pb2.WalkerControls(controls=control_list, scene=self.sceneID) # GrabSim_pb2.WalkerControls(controls=control_list, scene=self.sceneID)
# ) # )
@ -487,7 +497,7 @@ class Scene:
self.walker_control_generator(walkerID=control[0], autowalk=control[1], speed=control[2], X=control[3], self.walker_control_generator(walkerID=control[0], autowalk=control[1], speed=control[2], X=control[3],
Y=control[4], Yaw=control[5])) Y=control[4], Yaw=control[5]))
# 收集没有对话的统一控制 # 收集没有对话的统一控制
scene = stub.ControlWalkers( scene = self.stub.ControlWalkers(
GrabSim_pb2.WalkerControls(controls=control_list, scene=self.sceneID) GrabSim_pb2.WalkerControls(controls=control_list, scene=self.sceneID)
) )
return scene return scene
@ -502,7 +512,7 @@ class Scene:
is_autowalk = is_autowalk is_autowalk = is_autowalk
pose = GrabSim_pb2.Pose(X=loc[0], Y=loc[1], Yaw=180) pose = GrabSim_pb2.Pose(X=loc[0], Y=loc[1], Yaw=180)
controls.append(GrabSim_pb2.WalkerControls.WControl(id=i, autowalk=is_autowalk, speed=80, pose=pose)) controls.append(GrabSim_pb2.WalkerControls.WControl(id=i, autowalk=is_autowalk, speed=80, pose=pose))
scene = stub.ControlWalkers(GrabSim_pb2.WalkerControls(controls=controls, scene=self.sceneID)) scene = self.stub.ControlWalkers(GrabSim_pb2.WalkerControls(controls=controls, scene=self.sceneID))
return scene return scene
def control_walker_ls(self, walker_loc=[[-55, 750], [70, -200], [250, 1200], [0, 880]]): def control_walker_ls(self, walker_loc=[[-55, 750], [70, -200], [250, 1200], [0, 880]]):
@ -522,12 +532,12 @@ class Scene:
elif len(walker) == 6: elif len(walker) == 6:
self.control_walker(walker[0], walker[1], walker[2], walker[3], walker[4], walker[5]) self.control_walker(walker[0], walker[1], walker[2], walker[3], walker[4], walker[5])
# self.control_walker() # self.control_walker()
# scene = stub.ControlWalkers(GrabSim_pb2.WalkerControls(controls=controls, scene=self.sceneID)) # scene = self.stub.ControlWalkers(GrabSim_pb2.WalkerControls(controls=controls, scene=self.sceneID))
# return scene # return scene
return return
def control_joints(self, angles): def control_joints(self, angles):
stub.Do( self.stub.Do(
GrabSim_pb2.Action( GrabSim_pb2.Action(
scene=self.sceneID, scene=self.sceneID,
action=GrabSim_pb2.Action.ActionType.RotateJoints, action=GrabSim_pb2.Action.ActionType.RotateJoints,
@ -538,7 +548,7 @@ class Scene:
def add_object(self, type, X, Y, Z, Yaw=0): def add_object(self, type, X, Y, Z, Yaw=0):
if self.use_offset: if self.use_offset:
X, Y = X + loc_offset[0], Y + loc_offset[1] X, Y = X + loc_offset[0], Y + loc_offset[1]
stub.AddObjects( self.stub.AddObjects(
GrabSim_pb2.ObjectList( GrabSim_pb2.ObjectList(
objects=[ objects=[
GrabSim_pb2.ObjectList.Object(x=X, y=Y, yaw=Yaw, z=Z, type=type) GrabSim_pb2.ObjectList.Object(x=X, y=Y, yaw=Yaw, z=Z, type=type)
@ -554,13 +564,13 @@ class Scene:
else: else:
for objectID in args: for objectID in args:
remove_list.append(objectID) remove_list.append(objectID)
stub.RemoveObjects(GrabSim_pb2.RemoveList(IDs=remove_list, scene=self.sceneID)) self.stub.RemoveObjects(GrabSim_pb2.RemoveList(IDs=remove_list, scene=self.sceneID))
def clean_object(self): def clean_object(self):
stub.CleanObjects(GrabSim_pb2.SceneID(value=self.sceneID)) self.stub.CleanObjects(GrabSim_pb2.SceneID(value=self.sceneID))
def grasp(self, handID, objectID): def grasp(self, handID, objectID):
stub.Do( self.stub.Do(
GrabSim_pb2.Action( GrabSim_pb2.Action(
scene=self.sceneID, scene=self.sceneID,
action=GrabSim_pb2.Action.ActionType.Grasp, action=GrabSim_pb2.Action.ActionType.Grasp,
@ -569,7 +579,7 @@ class Scene:
) )
def release(self, handID): def release(self, handID):
stub.Do( self.stub.Do(
GrabSim_pb2.Action( GrabSim_pb2.Action(
scene=self.sceneID, scene=self.sceneID,
action=GrabSim_pb2.Action.ActionType.Release, action=GrabSim_pb2.Action.ActionType.Release,
@ -578,7 +588,7 @@ class Scene:
) )
def get_camera_color(self, image_only=True): def get_camera_color(self, image_only=True):
camera_data = stub.Capture( camera_data = self.stub.Capture(
GrabSim_pb2.CameraList( GrabSim_pb2.CameraList(
cameras=[GrabSim_pb2.CameraName.Head_Color], scene=self.sceneID cameras=[GrabSim_pb2.CameraName.Head_Color], scene=self.sceneID
) )
@ -589,7 +599,7 @@ class Scene:
return camera_data return camera_data
def get_camera_depth(self, image_only=True): def get_camera_depth(self, image_only=True):
camera_data = stub.Capture( camera_data = self.stub.Capture(
GrabSim_pb2.CameraList( GrabSim_pb2.CameraList(
cameras=[GrabSim_pb2.CameraName.Head_Depth], scene=self.sceneID cameras=[GrabSim_pb2.CameraName.Head_Depth], scene=self.sceneID
) )
@ -600,7 +610,7 @@ class Scene:
return camera_data return camera_data
def get_camera_segment(self, show=True): def get_camera_segment(self, show=True):
camera_data = stub.Capture( camera_data = self.stub.Capture(
GrabSim_pb2.CameraList( GrabSim_pb2.CameraList(
cameras=[GrabSim_pb2.CameraName.Head_Segment], scene=self.sceneID cameras=[GrabSim_pb2.CameraName.Head_Segment], scene=self.sceneID
) )
@ -611,7 +621,7 @@ class Scene:
return camera_data return camera_data
def chat_bubble(self, message): def chat_bubble(self, message):
stub.ControlRobot( self.stub.ControlRobot(
GrabSim_pb2.ControlInfo( GrabSim_pb2.ControlInfo(
scene=self.sceneID, type=0, action=1, content=message.strip() scene=self.sceneID, type=0, action=1, content=message.strip()
) )
@ -635,7 +645,7 @@ class Scene:
# def control_robot_action(self, scene_id=0, type=0, action=0, message="你好"): # def control_robot_action(self, scene_id=0, type=0, action=0, message="你好"):
# print('------------------control_robot_action----------------------') # print('------------------control_robot_action----------------------')
# scene = stub.ControlRobot( # scene = self.stub.ControlRobot(
# GrabSim_pb2.ControlInfo(scene=scene_id, type=type, action=action, content=message)) # GrabSim_pb2.ControlInfo(scene=scene_id, type=type, action=action, content=message))
# if (str(scene.info).find("Action Success") > -1): # if (str(scene.info).find("Action Success") > -1):
# print(scene.info) # print(scene.info)
@ -646,19 +656,19 @@ class Scene:
def animation_control(self, animation_type): def animation_control(self, animation_type):
# animation_type: 1:make coffee 2: pour water 3: grab food 4: mop floor 5: clean table # animation_type: 1:make coffee 2: pour water 3: grab food 4: mop floor 5: clean table
scene = stub.ControlRobot( scene = self.stub.ControlRobot(
GrabSim_pb2.ControlInfo(scene=self.sceneID, type=animation_type, action=1) GrabSim_pb2.ControlInfo(scene=self.sceneID, type=animation_type, action=1)
) )
if scene.info == "action success": if scene.info == "action success":
for i in range(2, animation_step[animation_type - 1] + 1): for i in range(2, animation_step[animation_type - 1] + 1):
stub.ControlRobot( self.stub.ControlRobot(
GrabSim_pb2.ControlInfo( GrabSim_pb2.ControlInfo(
scene=self.sceneID, type=animation_type, action=i scene=self.sceneID, type=animation_type, action=i
) )
) )
def animation_reset(self): def animation_reset(self):
stub.ControlRobot(GrabSim_pb2.ControlInfo(scene=self.sceneID, type=0, action=0)) self.stub.ControlRobot(GrabSim_pb2.ControlInfo(scene=self.sceneID, type=0, action=0))
# 手指移动到指定位置 # 手指移动到指定位置
def ik_control_joints(self, handNum=2, x=30, y=40, z=80): def ik_control_joints(self, handNum=2, x=30, y=40, z=80):
@ -668,7 +678,7 @@ class Scene:
GrabSim_pb2.HandPostureInfos.HandPostureObject(handNum=handNum, x=x, y=y, z=z, roll=0, pitch=0, yaw=0), GrabSim_pb2.HandPostureInfos.HandPostureObject(handNum=handNum, x=x, y=y, z=z, roll=0, pitch=0, yaw=0),
# GrabSim_pb2.HandPostureInfos.HandPostureObject(handNum=1, x=0, y=0, z=0, roll=0, pitch=0, yaw=0), # GrabSim_pb2.HandPostureInfos.HandPostureObject(handNum=1, x=0, y=0, z=0, roll=0, pitch=0, yaw=0),
] ]
temp = stub.GetIKControlInfos( temp = self.stub.GetIKControlInfos(
GrabSim_pb2.HandPostureInfos(scene=self.sceneID, handPostureObjects=HandPostureObject)) GrabSim_pb2.HandPostureInfos(scene=self.sceneID, handPostureObjects=HandPostureObject))
def move_to_obj(self, obj_id): def move_to_obj(self, obj_id):
@ -680,7 +690,7 @@ class Scene:
# value[i] = self.status.joints[i].angle # value[i] = self.status.joints[i].angle
# value[5] = 0 # value[5] = 0
# action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value) # action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
# scene = stub.Do(action) # scene = self.stub.Do(action)
# time.sleep(1.0) # time.sleep(1.0)
obj_info = scene.objects[obj_id] obj_info = scene.objects[obj_id]
@ -697,7 +707,7 @@ class Scene:
self.navigator.navigate(goal=(walk_v[0], walk_v[1]), animation=False) self.navigator.navigate(goal=(walk_v[0], walk_v[1]), animation=False)
else: else:
action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v)
scene = stub.Do(action) scene = self.stub.Do(action)
print("After Walk Position:", [scene.location.X, scene.location.Y, scene.rotation.Yaw]) print("After Walk Position:", [scene.location.X, scene.location.Y, scene.rotation.Yaw])
# 移动到进行操作任务的指定地点 # 移动到进行操作任务的指定地点
@ -710,7 +720,7 @@ class Scene:
# value[i] = self.status.joints[i].angle # value[i] = self.status.joints[i].angle
# value[5] = 0 # value[5] = 0
# action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value) # action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
# scene = stub.Do(action) # scene = self.stub.Do(action)
# time.sleep(1.0) # time.sleep(1.0)
cur_pos = [scene.location.X, scene.location.Y, scene.rotation.Yaw] cur_pos = [scene.location.X, scene.location.Y, scene.rotation.Yaw]
@ -745,12 +755,12 @@ class Scene:
walk_v[2] = 130 walk_v[2] = 130
# 移动到目标位置 # 移动到目标位置
action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v)
scene = stub.Do(action) scene = self.stub.Do(action)
print("After Walk Position:", [scene.location.X, scene.location.Y, scene.rotation.Yaw]) print("After Walk Position:", [scene.location.X, scene.location.Y, scene.rotation.Yaw])
# 相应的行动，由主办方封装 # 相应的行动，由主办方封装
def control_robot_action(self, type=0, action=0, message="你好"): def control_robot_action(self, type=0, action=0, message="你好"):
scene = stub.ControlRobot( scene = self.stub.ControlRobot(
GrabSim_pb2.ControlInfo( GrabSim_pb2.ControlInfo(
scene=self.sceneID, type=type, action=action, content=message scene=self.sceneID, type=type, action=action, content=message
) )
@ -770,7 +780,7 @@ class Scene:
value[i] = self.status.joints[i].angle value[i] = self.status.joints[i].angle
value[5] = 30 value[5] = 30
action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value) action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
scene = stub.Do(action) scene = self.stub.Do(action)
time.sleep(1.0) time.sleep(1.0)
if self.take_picture: if self.take_picture:
@ -806,7 +816,7 @@ class Scene:
GrabSim_pb2.ObjectList.Object(x=-115, y=280, z=85, roll=0, pitch=0, yaw=90, type=35), # Chess GrabSim_pb2.ObjectList.Object(x=-115, y=280, z=85, roll=0, pitch=0, yaw=90, type=35), # Chess
] ]
scene = stub.AddObjects(GrabSim_pb2.ObjectList(objects=obj_list, scene=self.sceneID)) scene = self.stub.AddObjects(GrabSim_pb2.ObjectList(objects=obj_list, scene=self.sceneID))
time.sleep(1.0) time.sleep(1.0)
# 实现抓握操作 # 实现抓握操作
@ -820,7 +830,7 @@ class Scene:
value[i] = self.status.joints[i].angle value[i] = self.status.joints[i].angle
value[5] = 30 value[5] = 30
action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value) action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
scene = stub.Do(action) scene = self.stub.Do(action)
time.sleep(1.0) time.sleep(1.0)
if self.take_picture: if self.take_picture:
@ -833,7 +843,7 @@ class Scene:
# obj_y -= 1 # obj_y -= 1
# values = [0,0,0,0,0, 10,-25,-45,-45,-45] # values = [0,0,0,0,0, 10,-25,-45,-45,-45]
# values= [-6, 0, 0, 0, 0, -6, 0, 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)) # self.stub.Do(GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Finger, values=values))
pass pass
if obj_info.name == "Glass": if obj_info.name == "Glass":
pass pass
@ -844,7 +854,7 @@ class Scene:
print('------------------grasp_obj----------------------') print('------------------grasp_obj----------------------')
action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Grasp, action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Grasp,
values=[hand_id, obj_id]) values=[hand_id, obj_id])
scene = stub.Do(action) scene = self.stub.Do(action)
time.sleep(3.0) time.sleep(3.0)
return True return True
@ -852,12 +862,12 @@ class Scene:
def robo_recover(self): def robo_recover(self):
action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, # 恢复原位 action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, # 恢复原位
values=[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) 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) scene = self.stub.Do(action)
# 恢复手指关节 # 恢复手指关节
def standard_finger(self): def standard_finger(self):
values = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0] 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)) self.stub.Do(GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Finger, values=values))
time.sleep(1.0) time.sleep(1.0)
# 弯腰以及手掌与放置面平齐 # 弯腰以及手掌与放置面平齐
@ -870,7 +880,7 @@ class Scene:
angle[20] = -30 angle[20] = -30
action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, # 弯腰 action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, # 弯腰
values=angle) values=angle)
scene = stub.Do(action) scene = self.stub.Do(action)
time.sleep(1.0) time.sleep(1.0)
# 实现放置操作 # 实现放置操作
@ -882,7 +892,7 @@ class Scene:
value[i] = self.status.joints[i].angle value[i] = self.status.joints[i].angle
value[5] = 30 value[5] = 30
action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value) action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
scene = stub.Do(action) scene = self.stub.Do(action)
time.sleep(1.0) time.sleep(1.0)
if self.take_picture: if self.take_picture:
@ -897,7 +907,7 @@ class Scene:
self.robo_stoop_parallel() self.robo_stoop_parallel()
print("------------------release_obj----------------------") print("------------------release_obj----------------------")
action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Release, values=[1]) action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Release, values=[1])
scene = stub.Do(action) scene = self.stub.Do(action)
time.sleep(2.0) time.sleep(2.0)
self.robo_recover() # 恢复肢体关节 self.robo_recover() # 恢复肢体关节
self.standard_finger() # 恢复手指关节 self.standard_finger() # 恢复手指关节
@ -942,12 +952,12 @@ class Scene:
walk_v = walk_v + [scene.rotation.Yaw - 90, 600, 100] walk_v = walk_v + [scene.rotation.Yaw - 90, 600, 100]
print("walk_v", walk_v) print("walk_v", walk_v)
action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v)
scene = stub.Do(action) scene = self.stub.Do(action)
print(scene.info) print(scene.info)
def navigation_move(self, plt, cur_objs, cur_obstacle_world_points, v_list, map_ratio, db, scene_id=0, map_id=11): def navigation_move(self, plt, cur_objs, cur_obstacle_world_points, v_list, map_ratio, db, scene_id=0, map_id=11):
print('------------------navigation_move----------------------') print('------------------navigation_move----------------------')
scene = stub.Observe(GrabSim_pb2.SceneID(value=scene_id)) scene = self.stub.Observe(GrabSim_pb2.SceneID(value=scene_id))
walk_value = [scene.location.X, scene.location.Y] walk_value = [scene.location.X, scene.location.Y]
print("position:", walk_value) print("position:", walk_value)
@ -958,7 +968,7 @@ class Scene:
walk_v = walk_value + [yaw, 250, 10] walk_v = walk_value + [yaw, 250, 10]
print("walk_v", walk_v) print("walk_v", walk_v)
action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v)
scene = stub.Do(action) scene = self.stub.Do(action)
# cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs, # cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs,
# objs_name_set) # objs_name_set)
@ -980,7 +990,7 @@ class Scene:
walk_v = walk_v + [yaw, 250, 10] walk_v = walk_v + [yaw, 250, 10]
print("walk_v", walk_v) print("walk_v", walk_v)
action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v)
scene = stub.Do(action) scene = self.stub.Do(action)
# cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs, # cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs,
# objs_name_set) # objs_name_set)
@ -1008,7 +1018,7 @@ class Scene:
return x, y return x, y
def explore(self, map, explore_range): def explore(self, map, explore_range):
scene = stub.Observe(GrabSim_pb2.SceneID(value=0)) scene = self.stub.Observe(GrabSim_pb2.SceneID(value=0))
cur_pos = [int(scene.location.X), int(scene.location.Y)] cur_pos = [int(scene.location.X), int(scene.location.Y)]
for i in range(cur_pos[0] - explore_range, cur_pos[0] + explore_range + 1): for i in range(cur_pos[0] - explore_range, cur_pos[0] + explore_range + 1):
for j in range(cur_pos[1] - explore_range, cur_pos[1] + explore_range + 1): for j in range(cur_pos[1] - explore_range, cur_pos[1] + explore_range + 1):
@ -1191,13 +1201,14 @@ class Scene:
cur_obstacle_world_points = [] cur_obstacle_world_points = []
obj_detect_count = 0 obj_detect_count = 0
walker_detect_count = 0 walker_detect_count = 0
fig = plt.figure()
object_pixels = {} object_pixels = {}
not_key_objs_id = {255, 254, 253, 107, 81} not_key_objs_id = {255, 254, 253, 107, 81}
img_data_segment,img_data_depth,img_data_color = self.get_cameras() img_data_segment,img_data_depth,img_data_color = self.get_cameras()
if len(img_data_segment.images) <1:
return
im_segment = img_data_segment.images[0] im_segment = img_data_segment.images[0]
im_depth = img_data_depth.images[0] im_depth = img_data_depth.images[0]
im_color = img_data_color.images[0] im_color = img_data_color.images[0]
@ -1218,10 +1229,7 @@ class Scene:
objs_id[251] = "walker" objs_id[251] = "walker"
# plt.imshow(d_depth, cmap="gray" if "depth" in im_depth.name.lower() else None) # plt.imshow(d_depth, cmap="gray" if "depth" in im_depth.name.lower() else None)
# plt.show() # plt.show()
plt.subplot(2, 2, 1) img_segment = d_segment
plt.imshow(d_segment, cmap="gray" if "depth" in im_segment.name.lower() else None)
plt.axis("off")
plt.title("相机分割")
d_depth = np.transpose(d_depth, (1, 0, 2)) d_depth = np.transpose(d_depth, (1, 0, 2))
@ -1281,14 +1289,24 @@ class Scene:
# world_point = self.transform_co(img_data_depth, pixel[0], pixel[1], d_depth[pixel[0]][pixel[1]][0], scene) # world_point = self.transform_co(img_data_depth, pixel[0], pixel[1], d_depth[pixel[0]][pixel[1]][0], scene)
# cur_obstacle_world_points.append([world_point[0], world_point[1]]) # cur_obstacle_world_points.append([world_point[0], world_point[1]])
# print(f"{pixel}：{[world_point[0], world_point[1]]}") # print(f"{pixel}：{[world_point[0], world_point[1]]}")
plt.subplot(2, 2, 2) img_obj = d_color
plt.imshow(d_color, cmap="gray" if "depth" in im_depth.name.lower() else None)
plt.axis('off')
plt.title("目标检测")
# self.ui_func(("draw_img","img_label_obj",d_color)) # self.ui_func(("draw_img","img_label_obj",d_color))
# 画分隔图
# plt.subplot(2, 2, 1)
plt.figure()
plt.imshow(img_segment, cmap="gray" if "depth" in im_segment.name.lower() else None)
plt.axis("off")
# plt.title("相机分割")
self.send_img("img_label_seg")
# 画目标检测图
# plt.subplot(2, 2, 2)
plt.figure()
plt.imshow(img_obj, cmap="gray" if "depth" in im_depth.name.lower() else None)
plt.axis('off')
# plt.title("目标检测")
for key, value in object_pixels.items(): for key, value in object_pixels.items():
if key == 0: if key == 0:
@ -1348,37 +1366,59 @@ class Scene:
ha='center', ha='center',
va='center') va='center')
plt.gca().add_patch(rect) plt.gca().add_patch(rect)
self.send_img("img_label_obj")
new_map = self.updateMap(cur_obstacle_world_points) new_map = self.updateMap(cur_obstacle_world_points)
self.draw_map(plt,new_map) self.draw_map(plt,new_map)
plt.subplot(2, 7, 14)
plt.axis("off") plt.axis("off")
plt.text(0, 0.9, f'检测行人数量：{walker_detect_count}', fontsize=10) self.send_img("img_label_map")
plt.text(0, 0.7, f'检测物体数量：{obj_detect_count}', fontsize=10)
plt.text(0, 0.5, f'新增语义信息：{walker_detect_count}', fontsize=10)
plt.text(0, 0.3, f'更新语义信息：{update_info_count}', fontsize=10)
# plt.subplot(2, 7, 14)
# plt.text(0, 0.9, f'检测行人数量：{walker_detect_count}', fontsize=10)
# plt.text(0, 0.7, f'检测物体数量：{obj_detect_count}', fontsize=10)
# plt.text(0, 0.5, f'新增语义信息：{walker_detect_count}', fontsize=10)
# plt.text(0, 0.3, f'更新语义信息：{update_info_count}', fontsize=10)
# plt.text(0, 0.1, f'已存语义信息：{self.infoCount}', fontsize=10) # plt.text(0, 0.1, f'已存语义信息：{self.infoCount}', fontsize=10)
output_path = os.path.join(self.output_path,"vision.png")
# canvas = FigureCanvas(plt.gcf()) # draw figures
# fig = plt.gcf() # 获取当前figure
# image = fig.canvas.print_to_buffer() # output_path = os.path.join(self.output_path,"vision.png")
# width, height = fig.get_size_inches() # 获取图像的宽度和高度（单位：英寸）
# dpi = fig.dpi # 获取图像的DPI
#
# width_px = int(width * dpi) # 转换为像素
# height_px = int(height * dpi) # 转换为像素
#
# image_pil = Image.frombuffer('RGB', (width, height), image, 'raw')
#
# # 转换为numpy数组 # # 转换为numpy数组
# image_np = np.asarray(image_pil) # image_np = np.asarray(image_pil)
self.ui_func(("draw_from_file","img_label_canvas",output_path)) # self.ui_func(("draw_from_file","img_label_canvas",output_path))
plt.close() # plt.close()
# plt.show() # plt.show()
# return cur_obstacle_world_points # return cur_obstacle_world_points
def send_img(self,name):
# 将图像保存到内存
buffer = io.BytesIO()
plt.savefig(buffer, bbox_inches='tight', pad_inches=0,format='png')
image_data = buffer.getvalue()
# 关闭当前图像
plt.close()
if not self.img_cache[name] == image_data:
self.img_cache[name] = image_data
self.ui_func(("draw_img",name,image_data))
def updateMap(self, points): def updateMap(self, points):
# map = copy.deepcopy(self.map_map) # map = copy.deepcopy(self.map_map)
map = copy.deepcopy(self.map_map_real) map = copy.deepcopy(self.map_map_real)
@ -1400,11 +1440,11 @@ class Scene:
return map return map
def draw_map(self,plt, map): def draw_map(self,plt, map):
plt.subplot(2, 1, 2) # 这里的2,1表示总共2行，1列，2表示这个位置是第2个子图 # plt.subplot(2, 1, 2) # 这里的2,1表示总共2行，1列，2表示这个位置是第2个子图
plt.imshow(map, cmap='binary', alpha=0.5, origin='lower', plt.imshow(map, cmap='binary', alpha=0.5, origin='lower',
extent=(-400 / self.map_ratio, 1450 / self.map_ratio, extent=(-400 / self.map_ratio, 1450 / self.map_ratio,
-350 / self.map_ratio, 600 / self.map_ratio)) -350 / self.map_ratio, 600 / self.map_ratio))
plt.title('可达性地图') # plt.title('可达性地图')
def get_id_object_world(self, id, scene): def get_id_object_world(self, id, scene):
pixels = [] pixels = []
@ -1435,11 +1475,11 @@ class Scene:
def get_cameras(self): def get_cameras(self):
# if self.time - self.last_camera_time > self.camera_interval: # if self.time - self.last_camera_time > self.camera_interval:
self.img_data_segment = get_camera([GrabSim_pb2.CameraName.Head_Segment]) self.img_data_segment = self.get_camera([GrabSim_pb2.CameraName.Head_Segment])
time.sleep(0.2) time.sleep(0.2)
self.img_data_depth = get_camera([GrabSim_pb2.CameraName.Head_Depth]) self.img_data_depth = self.get_camera([GrabSim_pb2.CameraName.Head_Depth])
time.sleep(0.2) time.sleep(0.2)
self.img_data_color = get_camera([GrabSim_pb2.CameraName.Head_Color]) self.img_data_color = self.get_camera([GrabSim_pb2.CameraName.Head_Color])
self.last_camera_time = self.time # self.last_camera_time = self.time
return self.img_data_segment,self.img_data_depth,self.img_data_color return self.img_data_segment,self.img_data_depth,self.img_data_color

0
robowaiter/utils/ui/__init__.py → robowaiter/scene/ui/__init__.py
View File

96
robowaiter/utils/ui/pyqt5.py → robowaiter/scene/ui/pyqt5.py
View File

@ -1,40 +1,52 @@
from PyQt5.QtWidgets import QApplication, QMainWindow, QLabel import importlib
from PyQt5.QtGui import QPixmap
from PyQt5.QtCore import Qt
from PyQt5.QtCore import QTimer, QCoreApplication
import sys
import os import os
from robowaiter.utils.ui.window import Ui_MainWindow from PyQt5.QtWidgets import QApplication, QMainWindow
from PyQt5.QtCore import QTimer
import sys
from robowaiter.scene.ui.window import Ui_MainWindow
from robowaiter.utils.basic import get_root_path from robowaiter.utils.basic import get_root_path
from PyQt5.QtCore import QThread from PyQt5.QtCore import QThread
import queue import queue
import numpy as np import numpy as np
from PyQt5.QtGui import QImage, QPixmap from PyQt5.QtGui import QImage, QPixmap
from PyQt5.QtCore import Qt from PyQt5.QtCore import Qt
from PyQt5.QtCore import QThread, pyqtSignal
# 创建线程安全的队列 from robowaiter.scene.ui.scene_ui import SceneUI
scene_queue = queue.Queue() from robowaiter.scene.ui import scene_ui
ui_queue = queue.Queue()
root_path = get_root_path() root_path = get_root_path()
class TaskThread(QThread): class TaskThread(QThread):
def __init__(self, task, scene_cls,robot_cls, *args, **kwargs): stop_signal = pyqtSignal()
def __init__(self, task, scene_cls,robot_cls,scene_queue,ui_queue, *args, **kwargs):
super(TaskThread, self).__init__(*args, **kwargs) super(TaskThread, self).__init__(*args, **kwargs)
self.task = task self.task = task
self.scene_cls = scene_cls self.scene_cls = scene_cls
self.robot_cls = robot_cls self.robot_cls = robot_cls
self.scene_queue = scene_queue
self.ui_queue = ui_queue
self.stoped = True
def run(self): def run(self):
self.task(self.scene_cls,self.robot_cls) self.scene = self.task(self.scene_cls,self.robot_cls,self.scene_queue,self.ui_queue)
self.scene._run()
# scene._run()
def run_scene(scene_cls,robot_cls): while not self.isInterruptionRequested():
self.scene.step()
# def stop(self):
# del self.scene
def run_scene(scene_cls,robot_cls,scene_queue,ui_queue):
scene = scene_cls(robot_cls(),scene_queue,ui_queue) scene = scene_cls(robot_cls(),scene_queue,ui_queue)
scene.reset() scene.reset()
scene.run() # scene.run()
# return scene
# try: # try:
# # 机器人系统的主循环 # # 机器人系统的主循环
# #
@ -42,22 +54,30 @@ def run_scene(scene_cls,robot_cls):
# # 打印异常信息到命令行 # # 打印异常信息到命令行
# print("Robot system error:", str(e)) # print("Robot system error:", str(e))
example_list = ("AEM","VLN","VLM",'GQA',"OT","AT","reset")
class UI(QMainWindow, Ui_MainWindow): class UI(QMainWindow, Ui_MainWindow):
scene = None scene = None
def __init__(self,scene_cls,robot_cls): def __init__(self,robot_cls):
app = QApplication(sys.argv) app = QApplication(sys.argv)
MainWindow = QMainWindow() MainWindow = QMainWindow()
super(UI, self).__init__(MainWindow) super(UI, self).__init__(MainWindow)
self.scene_cls = scene_cls # 创建线程安全的队列
self.scene_queue = queue.Queue()
self.ui_queue = queue.Queue()
self.scene_cls = SceneUI
self.robot_cls = robot_cls self.robot_cls = robot_cls
self.setupUi(MainWindow) # 初始化UI self.setupUi(MainWindow) # 初始化UI
# 初始化 # 绑定说话按钮
self.btn_say.clicked.connect(self.btn_say_on_click) self.btn_say.clicked.connect(self.btn_say_on_click)
# 绑定任务演示按钮
for example in example_list:
btn = getattr(self,f"btn_{example}")
btn.clicked.connect(self.create_example_click(example))
# 设置一个定时器，每隔100ms检查一次队列 # 设置一个定时器，每隔100ms检查一次队列
timer = QTimer() timer = QTimer()
@ -67,11 +87,24 @@ class UI(QMainWindow, Ui_MainWindow):
MainWindow.show() MainWindow.show()
thread = TaskThread(run_scene,scene_cls,robot_cls) self.thread = TaskThread(run_scene,self.scene_cls,self.robot_cls,self.scene_queue,self.ui_queue)
thread.start() self.thread.start()
sys.exit(app.exec_()) sys.exit(app.exec_())
def create_example_click(self,name):
def btn_example_on_click():
self.thread.requestInterruption()
self.thread.wait() # 等待线程安全退出
self.scene_queue = queue.Queue()
self.ui_queue = queue.Queue()
self.thread = TaskThread(run_scene, self.scene_cls, self.robot_cls,self.scene_queue,self.ui_queue)
self.thread.start()
self.scene_func((f"run_example",name))
return btn_example_on_click
def btn_say_on_click(self): def btn_say_on_click(self):
question = self.edit_say.text() question = self.edit_say.text()
if question[-1] == ")": if question[-1] == ")":
@ -83,11 +116,11 @@ class UI(QMainWindow, Ui_MainWindow):
# self.scene.customer_say("System", question) # self.scene.customer_say("System", question)
def scene_func(self,args): def scene_func(self,args):
scene_queue.put(args) self.scene_queue.put(args)
def handle_queue_messages(self): def handle_queue_messages(self):
while not ui_queue.empty(): while not self.ui_queue.empty():
message = ui_queue.get() message = self.ui_queue.get()
function_name = message[0] function_name = message[0]
function = getattr(self, function_name, None) function = getattr(self, function_name, None)
@ -116,13 +149,18 @@ class UI(QMainWindow, Ui_MainWindow):
def draw_img(self,control_name,img): def draw_img(self,control_name,img):
control = getattr(self,control_name,None) control = getattr(self,control_name,None)
# 加载并显示图片 # # 加载并显示图片
print(img) # print(img)
img = self.ndarray_to_qimage(img) # img = self.ndarray_to_qimage(img)
print(img) # print(img)
pixmap = QPixmap.fromImage(img) # 替换为你的图片路径
# image = Image.open(io.BytesIO(img))
# print(image)
pixmap = QPixmap.fromImage(QImage.fromData(img))
# self.label.setPixmap(pixmap) # self.label.setPixmap(pixmap)
control.setPixmap(self.scale_pixmap_to_label(pixmap, control)) control.setPixmap(self.scale_pixmap_to_label(pixmap, control))
# control.setPixmap(pixmap)
def draw_canvas(self,control_name,canvas): def draw_canvas(self,control_name,canvas):
control = getattr(self,control_name,None) control = getattr(self,control_name,None)

126
robowaiter/scene/ui/scene_ui.py Normal file
View File

@ -0,0 +1,126 @@
"""
UI场景
"""
import sys
from robowaiter.scene.scene import Scene
from robowaiter.utils.bt.draw import render_dot_tree
class SceneUI(Scene):
scene_queue = None
ui_queue = None
# camera_interval = 4
def __init__(self, robot,scene_queue,ui_queue):
self.scene_queue = scene_queue
self.ui_queue = ui_queue
super().__init__(robot)
# 在这里加入场景中发生的事件
self.take_picture = True
# while True:
# if not self.scene_queue.empty():
# param = self.scene_queue.get()
# # 处理参数...
# self.ui_queue.put(('say',"test"))
self.stoped = False
def run(self):
# 基类run
self._run()
# 运行并由robot打印每步信息
while not self.stoped:
self.step()
def run_example(self,example_name):
if example_name == 'VLN':
self.gen_obj()
self.add_walkers([
[29, 60, 520], # 顾客 0
[23, 0, 220], # 秃头老头子 1
[0, -55, 150], # 小男孩d走来走去 2
[10, -55, 750], # 3
[19, 70, -200], # 后门站着不动的 4
[21, 65, 1000, -90], # 大胖男占了一号桌 5
[5, 230, 1200], # 小女孩 6
[26, -28, -10, 90],
# [26, 60, 0, 90],
# [26, -28, 0, 90] , #在设置一个在后门随机游走的 7
# 设置为 26, 60, 0, 90]
[31, 280, 1200, -45] # 8
])
self.control_walker(2, True, 200, -55, 155, 90) # 飞速奔跑的小男孩
# self.control_walker(7, True, 80, -25, -150, 90)
self.control_walker(5, True, 65, 995, 520, 90)
self.control_walker(4, True, 65, 70, -200, 90)
self.new_event_list = [
(5, self.customer_say, (0, "请问哪里有空位啊？")),
(13, self.customer_say, (0, "我想坐高凳子。")),
(3, self.customer_say, (0, "你带我去吧。")),
(45, self.control_walker, (0, False, 100, -250, 480, -90)),
(-1, self.customer_say, (0, "谢谢你！这儿还不错！")),
]
if example_name == 'AEM':
pass
def init_robot(self):
# init robot
if self.robot:
self.robot.set_scene(self)
self.robot.load_BT()
self.draw_current_bt()
def draw_current_bt(self):
render_dot_tree(self.robot.bt.root,target_directory=self.output_path,name="current_bt")
self.ui_queue.put(('draw_from_file',"img_label_bt", f"{self.output_path}/current_bt.png"))
def ui_func(self,args):
# _,_,output_path = args
# plt.savefig(output_path)
self.ui_queue.put(args)
def _reset(self):
pass
def _step(self):
# print("已运行")
self.handle_queue_messages()
# if len(self.sub_task_seq.children) == 0:
# question = input("请输入指令：")
# if question[-1] == ")":
# print(f"设置目标:{question}")
# self.new_set_goal(question)
# else:
# self.customer_say("System",question)
def handle_queue_messages(self):
while not self.scene_queue.empty():
message = self.scene_queue.get()
function_name = message[0]
function = getattr(self, function_name, None)
args = []
if len(message)>1:
args = message[1:]
result = function(*args)
def stop(self):
self.stoped = True
if __name__ == '__main__':
from robowaiter.robot.robot import Robot
robot = Robot()
ui = UI( Robot)
# create task
# task = SceneUI(robot,ui)

0
robowaiter/utils/ui/ui.py → robowaiter/scene/ui/ui.py
View File

0
robowaiter/utils/ui/ui2py.bat → robowaiter/scene/ui/ui2py.bat
View File

121
robowaiter/scene/ui/window.py Normal file
View File

@ -0,0 +1,121 @@
# -*- coding: utf-8 -*-
# Form implementation generated from reading ui file 'window.ui'
#
# Created by: PyQt5 UI code generator 5.15.7
#
# WARNING: Any manual changes made to this file will be lost when pyuic5 is
# run again. Do not edit this file unless you know what you are doing.
from PyQt5 import QtCore, QtGui, QtWidgets
class Ui_MainWindow(object):
def setupUi(self, MainWindow):
MainWindow.setObjectName("MainWindow")
MainWindow.resize(960, 1080)
MainWindow.setAutoFillBackground(False)
self.centralwidget = QtWidgets.QWidget(MainWindow)
self.centralwidget.setObjectName("centralwidget")
self.edit_say = QtWidgets.QLineEdit(self.centralwidget)
self.edit_say.setGeometry(QtCore.QRect(430, 40, 221, 31))
self.edit_say.setObjectName("edit_say")
self.btn_say = QtWidgets.QPushButton(self.centralwidget)
self.btn_say.setGeometry(QtCore.QRect(680, 40, 75, 23))
self.btn_say.setObjectName("btn_say")
self.img_label_seg = QtWidgets.QLabel(self.centralwidget)
self.img_label_seg.setGeometry(QtCore.QRect(0, 200, 311, 241))
self.img_label_seg.setStyleSheet("border: 2px solid black;")
self.img_label_seg.setText("")
self.img_label_seg.setObjectName("img_label_seg")
self.img_label_bt = QtWidgets.QLabel(self.centralwidget)
self.img_label_bt.setGeometry(QtCore.QRect(0, 810, 811, 171))
self.img_label_bt.setStyleSheet("border: 2px solid black;")
self.img_label_bt.setText("")
self.img_label_bt.setObjectName("img_label_bt")
self.label_5 = QtWidgets.QLabel(self.centralwidget)
self.label_5.setGeometry(QtCore.QRect(370, 780, 111, 16))
self.label_5.setObjectName("label_5")
self.img_label_obj = QtWidgets.QLabel(self.centralwidget)
self.img_label_obj.setGeometry(QtCore.QRect(420, 200, 331, 241))
self.img_label_obj.setStyleSheet("border: 2px solid black;")
self.img_label_obj.setText("")
self.img_label_obj.setObjectName("img_label_obj")
self.img_label_map = QtWidgets.QLabel(self.centralwidget)
self.img_label_map.setGeometry(QtCore.QRect(0, 510, 471, 241))
self.img_label_map.setStyleSheet("border: 2px solid black;")
self.img_label_map.setText("")
self.img_label_map.setObjectName("img_label_map")
self.label_6 = QtWidgets.QLabel(self.centralwidget)
self.label_6.setGeometry(QtCore.QRect(180, 470, 111, 16))
self.label_6.setObjectName("label_6")
self.label_7 = QtWidgets.QLabel(self.centralwidget)
self.label_7.setGeometry(QtCore.QRect(130, 170, 111, 16))
self.label_7.setObjectName("label_7")
self.label_8 = QtWidgets.QLabel(self.centralwidget)
self.label_8.setGeometry(QtCore.QRect(570, 170, 111, 16))
self.label_8.setObjectName("label_8")
self.btn_AEM = QtWidgets.QPushButton(self.centralwidget)
self.btn_AEM.setGeometry(QtCore.QRect(30, 30, 121, 21))
self.btn_AEM.setObjectName("btn_AEM")
self.label_9 = QtWidgets.QLabel(self.centralwidget)
self.label_9.setGeometry(QtCore.QRect(20, 10, 331, 16))
self.label_9.setObjectName("label_9")
self.btn_VLN = QtWidgets.QPushButton(self.centralwidget)
self.btn_VLN.setGeometry(QtCore.QRect(30, 60, 121, 21))
self.btn_VLN.setObjectName("btn_VLN")
self.btn_VLM = QtWidgets.QPushButton(self.centralwidget)
self.btn_VLM.setGeometry(QtCore.QRect(30, 90, 121, 21))
self.btn_VLM.setObjectName("btn_VLM")
self.btn_GQA = QtWidgets.QPushButton(self.centralwidget)
self.btn_GQA.setGeometry(QtCore.QRect(160, 30, 121, 21))
self.btn_GQA.setObjectName("btn_GQA")
self.btn_OT = QtWidgets.QPushButton(self.centralwidget)
self.btn_OT.setGeometry(QtCore.QRect(160, 60, 121, 21))
self.btn_OT.setObjectName("btn_OT")
self.btn_AT = QtWidgets.QPushButton(self.centralwidget)
self.btn_AT.setGeometry(QtCore.QRect(160, 90, 121, 21))
self.btn_AT.setObjectName("btn_AT")
self.btn_reset = QtWidgets.QPushButton(self.centralwidget)
self.btn_reset.setGeometry(QtCore.QRect(30, 120, 251, 21))
self.btn_reset.setObjectName("btn_reset")
MainWindow.setCentralWidget(self.centralwidget)
self.menubar = QtWidgets.QMenuBar(MainWindow)
self.menubar.setGeometry(QtCore.QRect(0, 0, 960, 23))
self.menubar.setObjectName("menubar")
MainWindow.setMenuBar(self.menubar)
self.statusbar = QtWidgets.QStatusBar(MainWindow)
self.statusbar.setObjectName("statusbar")
MainWindow.setStatusBar(self.statusbar)
self.retranslateUi(MainWindow)
QtCore.QMetaObject.connectSlotsByName(MainWindow)
def retranslateUi(self, MainWindow):
_translate = QtCore.QCoreApplication.translate
MainWindow.setWindowTitle(_translate("MainWindow", "MainWindow"))
self.edit_say.setText(_translate("MainWindow", "Is(AC,On)"))
self.btn_say.setText(_translate("MainWindow", "说话"))
self.label_5.setText(_translate("MainWindow", "当前行为树"))
self.label_6.setText(_translate("MainWindow", "可达性地图"))
self.label_7.setText(_translate("MainWindow", "实例分割"))
self.label_8.setText(_translate("MainWindow", "目标检测"))
self.btn_AEM.setText(_translate("MainWindow", "环境主动探索"))
self.label_9.setText(_translate("MainWindow", "任务演示：（播放动画时需等待动画播放完毕才会重置场景）"))
self.btn_VLN.setText(_translate("MainWindow", "视觉语言导航"))
self.btn_VLM.setText(_translate("MainWindow", "视觉语言操作"))
self.btn_GQA.setText(_translate("MainWindow", "具身多轮对话"))
self.btn_OT.setText(_translate("MainWindow", "开放具身任务"))
self.btn_AT.setText(_translate("MainWindow", "自主具身任务"))
self.btn_reset.setText(_translate("MainWindow", "重置"))
if __name__ == "__main__":
import sys
app = QtWidgets.QApplication(sys.argv)
MainWindow = QtWidgets.QMainWindow()
ui = Ui_MainWindow()
ui.setupUi(MainWindow)
MainWindow.show()
sys.exit(app.exec_())

281
robowaiter/scene/ui/window.ui Normal file
View File

@ -0,0 +1,281 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>MainWindow</class>
<widget class="QMainWindow" name="MainWindow">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>960</width>
<height>1080</height>
</rect>
</property>
<property name="windowTitle">
<string>MainWindow</string>
</property>
<property name="autoFillBackground">
<bool>false</bool>
</property>
<widget class="QWidget" name="centralwidget">
<widget class="QLineEdit" name="edit_say">
<property name="geometry">
<rect>
<x>430</x>
<y>40</y>
<width>221</width>
<height>31</height>
</rect>
</property>
<property name="text">
<string>Is(AC,On)</string>
</property>
</widget>
<widget class="QPushButton" name="btn_say">
<property name="geometry">
<rect>
<x>680</x>
<y>40</y>
<width>75</width>
<height>23</height>
</rect>
</property>
<property name="text">
<string>说话</string>
</property>
</widget>
<widget class="QLabel" name="img_label_seg">
<property name="geometry">
<rect>
<x>0</x>
<y>200</y>
<width>311</width>
<height>241</height>
</rect>
</property>
<property name="styleSheet">
<string notr="true">border: 2px solid black;</string>
</property>
<property name="text">
<string/>
</property>
</widget>
<widget class="QLabel" name="img_label_bt">
<property name="geometry">
<rect>
<x>0</x>
<y>810</y>
<width>811</width>
<height>171</height>
</rect>
</property>
<property name="styleSheet">
<string notr="true">border: 2px solid black;</string>
</property>
<property name="text">
<string/>
</property>
</widget>
<widget class="QLabel" name="label_5">
<property name="geometry">
<rect>
<x>370</x>
<y>780</y>
<width>111</width>
<height>16</height>
</rect>
</property>
<property name="text">
<string>当前行为树</string>
</property>
</widget>
<widget class="QLabel" name="img_label_obj">
<property name="geometry">
<rect>
<x>420</x>
<y>200</y>
<width>331</width>
<height>241</height>
</rect>
</property>
<property name="styleSheet">
<string notr="true">border: 2px solid black;</string>
</property>
<property name="text">
<string/>
</property>
</widget>
<widget class="QLabel" name="img_label_map">
<property name="geometry">
<rect>
<x>0</x>
<y>510</y>
<width>471</width>
<height>241</height>
</rect>
</property>
<property name="styleSheet">
<string notr="true">border: 2px solid black;</string>
</property>
<property name="text">
<string/>
</property>
</widget>
<widget class="QLabel" name="label_6">
<property name="geometry">
<rect>
<x>180</x>
<y>470</y>
<width>111</width>
<height>16</height>
</rect>
</property>
<property name="text">
<string>可达性地图</string>
</property>
</widget>
<widget class="QLabel" name="label_7">
<property name="geometry">
<rect>
<x>130</x>
<y>170</y>
<width>111</width>
<height>16</height>
</rect>
</property>
<property name="text">
<string>实例分割</string>
</property>
</widget>
<widget class="QLabel" name="label_8">
<property name="geometry">
<rect>
<x>570</x>
<y>170</y>
<width>111</width>
<height>16</height>
</rect>
</property>
<property name="text">
<string>目标检测</string>
</property>
</widget>
<widget class="QPushButton" name="btn_AEM">
<property name="geometry">
<rect>
<x>30</x>
<y>30</y>
<width>121</width>
<height>21</height>
</rect>
</property>
<property name="text">
<string>环境主动探索</string>
</property>
</widget>
<widget class="QLabel" name="label_9">
<property name="geometry">
<rect>
<x>20</x>
<y>10</y>
<width>331</width>
<height>16</height>
</rect>
</property>
<property name="text">
<string>任务演示：（播放动画时需等待动画播放完毕才会重置场景）</string>
</property>
</widget>
<widget class="QPushButton" name="btn_VLN">
<property name="geometry">
<rect>
<x>30</x>
<y>60</y>
<width>121</width>
<height>21</height>
</rect>
</property>
<property name="text">
<string>视觉语言导航</string>
</property>
</widget>
<widget class="QPushButton" name="btn_VLM">
<property name="geometry">
<rect>
<x>30</x>
<y>90</y>
<width>121</width>
<height>21</height>
</rect>
</property>
<property name="text">
<string>视觉语言操作</string>
</property>
</widget>
<widget class="QPushButton" name="btn_GQA">
<property name="geometry">
<rect>
<x>160</x>
<y>30</y>
<width>121</width>
<height>21</height>
</rect>
</property>
<property name="text">
<string>具身多轮对话</string>
</property>
</widget>
<widget class="QPushButton" name="btn_OT">
<property name="geometry">
<rect>
<x>160</x>
<y>60</y>
<width>121</width>
<height>21</height>
</rect>
</property>
<property name="text">
<string>开放具身任务</string>
</property>
</widget>
<widget class="QPushButton" name="btn_AT">
<property name="geometry">
<rect>
<x>160</x>
<y>90</y>
<width>121</width>
<height>21</height>
</rect>
</property>
<property name="text">
<string>自主具身任务</string>
</property>
</widget>
<widget class="QPushButton" name="btn_reset">
<property name="geometry">
<rect>
<x>30</x>
<y>120</y>
<width>251</width>
<height>21</height>
</rect>
</property>
<property name="text">
<string>重置</string>
</property>
</widget>
</widget>
<widget class="QMenuBar" name="menubar">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>960</width>
<height>23</height>
</rect>
</property>
</widget>
<widget class="QStatusBar" name="statusbar"/>
</widget>
<resources/>
<connections/>
</ui>

88
robowaiter/utils/ui/scene_ui.py
View File

@ -1,88 +0,0 @@
"""
交互式场景，输入
"""
import tkinter as tk
from robowaiter.utils.ui.pyqt5 import UI
import os
from matplotlib import pyplot as plt
# todo: 接收点单信息，大模型生成任务规划
from robowaiter.scene.scene import Scene
from robowaiter.utils.ui.ui import RobotInterface
from robowaiter.utils.bt.draw import render_dot_tree
class SceneUI(Scene):
scene_queue = None
ui_queue = None
# camera_interval = 4
def __init__(self, robot,scene_queue,ui_queue):
self.scene_queue = scene_queue
self.ui_queue = ui_queue
super().__init__(robot)
# 在这里加入场景中发生的事件
self.take_picture = True
# while True:
# if not self.scene_queue.empty():
# param = self.scene_queue.get()
# # 处理参数...
# self.ui_queue.put(('say',"test"))
def init_robot(self, robot):
# init robot
self.robot = robot
if robot:
robot.set_scene(self)
robot.load_BT()
self.draw_current_bt()
def draw_current_bt(self):
render_dot_tree(self.robot.bt.root,target_directory=self.output_path,name="current_bt")
self.ui_queue.put(('draw_from_file',"img_label_bt", f"{self.output_path}/current_bt.png"))
def ui_func(self,args):
_,_,output_path = args
plt.savefig(output_path)
self.ui_queue.put(args)
def _reset(self):
pass
def _step(self):
# print("已运行")
self.handle_queue_messages()
# if len(self.sub_task_seq.children) == 0:
# question = input("请输入指令：")
# if question[-1] == ")":
# print(f"设置目标:{question}")
# self.new_set_goal(question)
# else:
# self.customer_say("System",question)
def handle_queue_messages(self):
while not self.scene_queue.empty():
message = self.scene_queue.get()
function_name = message[0]
function = getattr(self, function_name, None)
args = []
if len(message)>1:
args = message[1:]
result = function(*args)
if __name__ == '__main__':
from robowaiter.robot.robot import Robot
robot = Robot()
ui = UI(SceneUI, Robot)
# create task
# task = SceneUI(robot,ui)

67
robowaiter/utils/ui/window.py
View File

@ -1,67 +0,0 @@
# -*- coding: utf-8 -*-
# Form implementation generated from reading ui file 'window.ui'
#
# Created by: PyQt5 UI code generator 5.15.7
#
# WARNING: Any manual changes made to this file will be lost when pyuic5 is
# run again. Do not edit this file unless you know what you are doing.
from PyQt5 import QtCore, QtGui, QtWidgets
class Ui_MainWindow(object):
def setupUi(self, MainWindow):
MainWindow.setObjectName("MainWindow")
MainWindow.resize(960, 1080)
MainWindow.setAutoFillBackground(False)
self.centralwidget = QtWidgets.QWidget(MainWindow)
self.centralwidget.setObjectName("centralwidget")
self.edit_say = QtWidgets.QLineEdit(self.centralwidget)
self.edit_say.setGeometry(QtCore.QRect(430, 40, 221, 31))
self.edit_say.setObjectName("edit_say")
self.btn_say = QtWidgets.QPushButton(self.centralwidget)
self.btn_say.setGeometry(QtCore.QRect(680, 40, 75, 23))
self.btn_say.setObjectName("btn_say")
self.img_label_canvas = QtWidgets.QLabel(self.centralwidget)
self.img_label_canvas.setGeometry(QtCore.QRect(0, 160, 811, 441))
self.img_label_canvas.setStyleSheet("border: 2px solid black;")
self.img_label_canvas.setText("")
self.img_label_canvas.setObjectName("img_label_canvas")
self.img_label_bt = QtWidgets.QLabel(self.centralwidget)
self.img_label_bt.setGeometry(QtCore.QRect(0, 670, 811, 311))
self.img_label_bt.setStyleSheet("border: 2px solid black;")
self.img_label_bt.setText("")
self.img_label_bt.setObjectName("img_label_bt")
self.label_5 = QtWidgets.QLabel(self.centralwidget)
self.label_5.setGeometry(QtCore.QRect(400, 630, 111, 16))
self.label_5.setObjectName("label_5")
MainWindow.setCentralWidget(self.centralwidget)
self.menubar = QtWidgets.QMenuBar(MainWindow)
self.menubar.setGeometry(QtCore.QRect(0, 0, 960, 23))
self.menubar.setObjectName("menubar")
MainWindow.setMenuBar(self.menubar)
self.statusbar = QtWidgets.QStatusBar(MainWindow)
self.statusbar.setObjectName("statusbar")
MainWindow.setStatusBar(self.statusbar)
self.retranslateUi(MainWindow)
QtCore.QMetaObject.connectSlotsByName(MainWindow)
def retranslateUi(self, MainWindow):
_translate = QtCore.QCoreApplication.translate
MainWindow.setWindowTitle(_translate("MainWindow", "MainWindow"))
self.edit_say.setText(_translate("MainWindow", "Is(AC,On)"))
self.btn_say.setText(_translate("MainWindow", "说话"))
self.label_5.setText(_translate("MainWindow", "语义地图"))
if __name__ == "__main__":
import sys
app = QtWidgets.QApplication(sys.argv)
MainWindow = QtWidgets.QMainWindow()
ui = Ui_MainWindow()
ui.setupUi(MainWindow)
MainWindow.show()
sys.exit(app.exec_())

106
robowaiter/utils/ui/window.ui
View File

@ -1,106 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>MainWindow</class>
<widget class="QMainWindow" name="MainWindow">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>960</width>
<height>1080</height>
</rect>
</property>
<property name="windowTitle">
<string>MainWindow</string>
</property>
<property name="autoFillBackground">
<bool>false</bool>
</property>
<widget class="QWidget" name="centralwidget">
<widget class="QLineEdit" name="edit_say">
<property name="geometry">
<rect>
<x>430</x>
<y>40</y>
<width>221</width>
<height>31</height>
</rect>
</property>
<property name="text">
<string>Is(AC,On)</string>
</property>
</widget>
<widget class="QPushButton" name="btn_say">
<property name="geometry">
<rect>
<x>680</x>
<y>40</y>
<width>75</width>
<height>23</height>
</rect>
</property>
<property name="text">
<string>说话</string>
</property>
</widget>
<widget class="QLabel" name="img_label_canvas">
<property name="geometry">
<rect>
<x>0</x>
<y>160</y>
<width>811</width>
<height>441</height>
</rect>
</property>
<property name="styleSheet">
<string notr="true">border: 2px solid black;</string>
</property>
<property name="text">
<string/>
</property>
</widget>
<widget class="QLabel" name="img_label_bt">
<property name="geometry">
<rect>
<x>0</x>
<y>670</y>
<width>811</width>
<height>311</height>
</rect>
</property>
<property name="styleSheet">
<string notr="true">border: 2px solid black;</string>
</property>
<property name="text">
<string/>
</property>
</widget>
<widget class="QLabel" name="label_5">
<property name="geometry">
<rect>
<x>400</x>
<y>630</y>
<width>111</width>
<height>16</height>
</rect>
</property>
<property name="text">
<string>语义地图</string>
</property>
</widget>
</widget>
<widget class="QMenuBar" name="menubar">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>960</width>
<height>23</height>
</rect>
</property>
</widget>
<widget class="QStatusBar" name="statusbar"/>
</widget>
<resources/>
<connections/>
</ui>

5
run_ui.py Normal file
View File

@ -0,0 +1,5 @@
from robowaiter.scene.ui.pyqt5 import UI
from robowaiter.robot.robot import Robot
robot = Robot()
ui = UI( Robot)