Merge branch 'main' of github.com:HPCL-EI/RoboWaiter
This commit is contained in:
ChenXL97
commit
95abf77d32
|
|
@ -30,6 +30,17 @@ class Bahavior(ptree.behaviour.Behaviour):
|
|||
# all_object = set()
|
||||
# all_place=set()
|
||||
|
||||
num_of_obj_on_place={
|
||||
'Bar': 0, # (247.0, 520.0, 100.0)
|
||||
'Bar2': 0,
|
||||
'WaterTable': 0,
|
||||
'CoffeeTable': 0,
|
||||
'Table1': 0,
|
||||
'Table2': 0,
|
||||
'Table3': 0,
|
||||
'BrightTable6': 0,
|
||||
}
|
||||
|
||||
place_xyz_dic={
|
||||
'Bar': (247.0, 520.0, 100.0), #(247.0, 520.0, 100.0)
|
||||
'Bar2': (240.0, 40.0, 100.0),
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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"]
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -33,8 +33,16 @@ class PutDown(Act):
|
|||
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]
|
||||
|
||||
if Act.num_of_obj_on_place[self.target_place]>=1:
|
||||
release_pos[1] += 25
|
||||
|
||||
Act.num_of_obj_on_place[self.target_place]+=1
|
||||
|
||||
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"]
|
||||
|
|
|
|||
|
|
@ -61,13 +61,13 @@ class Turn(Act):
|
|||
info["add"] = {f'Is({arg[0]},Off)'}
|
||||
info["del_set"] = {f'Is({arg[0]},On)'}
|
||||
elif arg[0]=='ACTemperature':
|
||||
info["pre"] = {f'Holding(Nothing)'}
|
||||
info["pre"] = {f'Holding(Nothing)',f'Is(AC,On)'}
|
||||
if arg[1]=="Up":
|
||||
info["pre"] |= {f'Is({arg[0]},Down)'}
|
||||
# info["pre"] |= {f'Is({arg[0]},Down)'}
|
||||
info["add"] = {f'Is({arg[0]},Up)'}
|
||||
info["del_set"] = {f'Is({arg[0]},Down)'}
|
||||
elif arg[1]=="Down":
|
||||
info["pre"] |= {f'Is({arg[0]},Up)'}
|
||||
# info["pre"] |= {f'Is({arg[0]},Up)'}
|
||||
info["add"] = {f'Is({arg[0]},Down)'}
|
||||
info["del_set"] = {f'Is({arg[0]},Up)'}
|
||||
return info
|
||||
|
|
@ -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"]
|
||||
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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']
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -14,6 +14,8 @@ class NewCustomer(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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -6,7 +6,7 @@
|
|||
create_sub_task
|
||||
{"goal":"On(Coffee,CoffeeTable)"}00
|
||||
|
||||
不用了
|
||||
不用了。
|
||||
好的，您有需要再跟我说
|
||||
stop_serve
|
||||
{}
|
||||
|
|
@ -91,8 +91,26 @@ create_sub_task
|
|||
{"goal":"Is(AC,On),Is(Curtain,Off)"}
|
||||
|
||||
|
||||
下班啦！别忘了打扫卫生。
|
||||
收到！请问您需要来点什么吗？
|
||||
create_sub_task
|
||||
"{""Is(Floor,Clean)"",""Is(Table1,Clean)"",""Is(Chairs,Clean)"",""Is(AC,Off)"",""Is(HallLight,Off)"",""Is(TubeLight,Off)"",""Is(Curtain,Off)""}"
|
||||
|
||||
|
||||
请问洗手间在哪里？
|
||||
洗手间在这附近
|
||||
get_object_info
|
||||
{"obj":"洗手间"}
|
||||
|
||||
|
||||
|
||||
给我来杯酸奶和冰红茶，我坐在对面的桌子那儿。
|
||||
好的，请稍等。
|
||||
create_sub_task
|
||||
{"goal":"On(BottledDrink,WaterTable),On(Yogurt,WaterTable)"}
|
||||
|
||||
|
||||
好热呀，想开空调，想要温度调低点！
|
||||
抱歉，我马上去开空调并调低空调温度。
|
||||
create_sub_task
|
||||
{"goal":"Is(ACTemperature,Down)"}
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@ objects_dic = {}
|
|||
obstacle_objs_id = [114, 115, 122, 96, 102, 83, 121, 105, 108, 89, 100, 90,
|
||||
111, 103, 95, 92, 76, 113, 101, 29, 112, 87, 109, 98,
|
||||
106, 120, 97, 86, 104, 78, 85, 81, 82, 84, 91, 93, 94,
|
||||
99, 107, 116, 117, 118, 119, 255]
|
||||
99, 107, 116, 117, 118, 119, 255, 251]
|
||||
not_key_objs_id = {255,254,253,107,81}
|
||||
|
||||
'''
|
||||
|
|
@ -367,12 +367,11 @@ def get_id_object_pixels(id, scene):
|
|||
world_points.append(transform_co(img_data_depth, pixel[0], pixel[1], d_depth[pixel[0]][pixel[1]][0], scene))
|
||||
return world_points
|
||||
|
||||
|
||||
|
||||
|
||||
def get_obstacle_point(db, scene, cur_obstacle_world_points, map_ratio):
|
||||
def get_obstacle_point(plt, db, scene, cur_obstacle_world_points, map_ratio):
|
||||
cur_obstacle_pixel_points = []
|
||||
object_pixels = {}
|
||||
obj_detect_count = 0
|
||||
cur_objs_id = []
|
||||
colors = [
|
||||
'red',
|
||||
'pink',
|
||||
|
|
@ -403,10 +402,13 @@ def get_obstacle_point(db, scene, cur_obstacle_world_points, map_ratio):
|
|||
for item in items:
|
||||
key, value = item.split(":")
|
||||
objs_id[int(key)] = value
|
||||
objs_id[251] = "walker"
|
||||
# plt.imshow(d_depth, cmap="gray" if "depth" in im_depth.name.lower() else None)
|
||||
# plt.show()
|
||||
#
|
||||
# plt.imshow(d_segment, cmap="gray" if "depth" in im_segment.name.lower() else None)
|
||||
plt.subplot(2, 2, 1)
|
||||
plt.imshow(d_segment, cmap="gray" if "depth" in im_segment.name.lower() else None)
|
||||
plt.axis("off")
|
||||
plt.title("相机分割")
|
||||
# plt.show()
|
||||
|
||||
d_depth = np.transpose(d_depth, (1, 0, 2))
|
||||
|
|
@ -439,16 +441,21 @@ def get_obstacle_point(db, scene, cur_obstacle_world_points, map_ratio):
|
|||
world_point = 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]])
|
||||
# print(f"{pixel}：{[world_point[0], world_point[1]]}")
|
||||
|
||||
plt.subplot(2, 2, 2)
|
||||
plt.imshow(d_color, cmap="gray" if "depth" in im_depth.name.lower() else None)
|
||||
plt.axis('off')
|
||||
plt.title("目标检测")
|
||||
# plt.tight_layout()
|
||||
|
||||
for key, value in object_pixels.items():
|
||||
if key == 101 or key == 0:
|
||||
if key == 0 or key not in objs_id.keys():
|
||||
continue
|
||||
if key in [91, 84]:
|
||||
|
||||
if key in [91, 84, 96, 87, 102, 106, 120, 85,113, 101, 83, 251]:
|
||||
X = np.array(value)
|
||||
db.fit(X)
|
||||
labels = db.labels_
|
||||
|
||||
# 将数据按照聚类标签分组，并打印每个分组的数据
|
||||
for i in range(max(labels) + 1): # 从0到最大聚类标签的值
|
||||
group_data = X[labels == i] # 获取当前标签的数据
|
||||
|
|
@ -458,6 +465,9 @@ def get_obstacle_point(db, scene, cur_obstacle_world_points, map_ratio):
|
|||
y_min = min(p[1] for p in group_data)
|
||||
if x_max - x_min < 10 or y_max - y_min < 10:
|
||||
continue
|
||||
if key != 251:
|
||||
obj_detect_count += 1
|
||||
cur_objs_id.append(objs_id[key])
|
||||
# 在指定的位置绘制方框
|
||||
# 创建矩形框
|
||||
rect = patches.Rectangle((x_min, y_min), (x_max - x_min), (y_max - y_min), linewidth=1, edgecolor=colors[key % 10],
|
||||
|
|
@ -465,7 +475,11 @@ def get_obstacle_point(db, scene, cur_obstacle_world_points, map_ratio):
|
|||
plt.text(x_min, y_min, f'{objs_id[key]}', fontdict={'family': 'serif', 'size': 10, 'color': 'green'}, ha='center',
|
||||
va='center')
|
||||
plt.gca().add_patch(rect)
|
||||
|
||||
else:
|
||||
if key != 251:
|
||||
obj_detect_count += 1
|
||||
cur_objs_id.append(objs_id[key])
|
||||
x_max = max(p[0] for p in value)
|
||||
y_max = max(p[1] for p in value)
|
||||
x_min = min(p[0] for p in value)
|
||||
|
|
@ -484,11 +498,12 @@ def get_obstacle_point(db, scene, cur_obstacle_world_points, map_ratio):
|
|||
# rect = patches.Rectangle((0, 255), 15, 30, linewidth=1, edgecolor='g',
|
||||
# facecolor='none')
|
||||
|
||||
plt.subplot(2, 7, 14) # 这里的2,1表示总共2行，1列，2表示这个位置是第2个子图
|
||||
|
||||
# 将矩形框添加到图像中
|
||||
# plt.gca().add_patch(rect)
|
||||
plt.show()
|
||||
return cur_obstacle_world_points
|
||||
plt.text(0, 0.7, f'检测物体数量：{obj_detect_count}', fontsize=10)
|
||||
|
||||
# plt.show()
|
||||
return cur_obstacle_world_points, cur_objs_id
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
File diff suppressed because one or more lines are too long
|
|
@ -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):
|
||||
|
|
|
|||
|
|
@ -4,6 +4,8 @@ import time
|
|||
import grpc
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from matplotlib import patches
|
||||
|
||||
from robowaiter.proto import camera
|
||||
from robowaiter.proto import semantic_map
|
||||
from robowaiter.algos.navigator.navigate import Navigator
|
||||
|
|
@ -13,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(
|
||||
|
|
@ -32,6 +40,11 @@ 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):
|
||||
print('------------------show_image----------------------')
|
||||
|
|
@ -100,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:
|
||||
|
|
@ -568,7 +585,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
|
||||
|
|
@ -587,6 +614,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: # 开关窗帘不需要移动
|
||||
|
|
@ -634,6 +671,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
|
||||
|
|
@ -664,6 +710,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":
|
||||
|
|
@ -713,6 +772,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)
|
||||
|
|
@ -766,7 +838,7 @@ class Scene:
|
|||
scene = stub.Do(action)
|
||||
print(scene.info)
|
||||
|
||||
def navigation_move(self, cur_objs, objs_name_set, 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----------------------')
|
||||
scene = stub.Observe(GrabSim_pb2.SceneID(value=scene_id))
|
||||
walk_value = [scene.location.X, scene.location.Y]
|
||||
|
|
@ -779,10 +851,10 @@ class Scene:
|
|||
print("walk_v", walk_v)
|
||||
action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v)
|
||||
scene = stub.Do(action)
|
||||
cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs,
|
||||
objs_name_set)
|
||||
# cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs,
|
||||
# objs_name_set)
|
||||
|
||||
cur_obstacle_world_points = camera.get_obstacle_point(db, scene, cur_obstacle_world_points,map_ratio)
|
||||
cur_obstacle_world_points, cur_objs_id= camera.get_obstacle_point(plt, db, scene, cur_obstacle_world_points,map_ratio)
|
||||
|
||||
|
||||
# if scene.info == "Unreachable":
|
||||
|
|
@ -801,15 +873,15 @@ class Scene:
|
|||
action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v)
|
||||
scene = stub.Do(action)
|
||||
|
||||
cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs,
|
||||
objs_name_set)
|
||||
# cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs,
|
||||
# objs_name_set)
|
||||
|
||||
cur_obstacle_world_points = camera.get_obstacle_point(db, scene, cur_obstacle_world_points, map_ratio)
|
||||
cur_obstacle_world_points, cur_objs_id= camera.get_obstacle_point(plt, db, scene, cur_obstacle_world_points, map_ratio)
|
||||
|
||||
|
||||
# if scene.info == "Unreachable":
|
||||
print(scene.info)
|
||||
return cur_objs, objs_name_set, cur_obstacle_world_points
|
||||
return cur_obstacle_world_points, cur_objs_id
|
||||
|
||||
def isOutMap(self, pos, min_x=-200, max_x=600, min_y=-250, max_y=1300):
|
||||
if pos[0] <= min_x or pos[0] >= max_x or pos[1] <= min_y or pos[1] >= max_y:
|
||||
|
|
@ -905,3 +977,235 @@ class Scene:
|
|||
return True
|
||||
else:
|
||||
return False
|
||||
|
||||
|
||||
|
||||
@staticmethod
|
||||
def transform_co(img_data, pixel_x_, pixel_y_, depth_, scene, id=0, label=0):
|
||||
im = img_data.images[0]
|
||||
|
||||
# 相机外参矩阵
|
||||
out_matrix = np.array(im.parameters.matrix).reshape((4, 4))
|
||||
|
||||
d = np.frombuffer(im.data, dtype=im.dtype).reshape((im.height, im.width, im.channels))
|
||||
depth = depth_
|
||||
|
||||
# 将像素坐标转换为归一化设备坐标
|
||||
normalized_x = (pixel_x_ - im.parameters.cx) / im.parameters.fx
|
||||
normalized_y = (pixel_y_ - im.parameters.cy) / im.parameters.fy
|
||||
|
||||
# 将归一化设备坐标和深度值转换为相机坐标
|
||||
camera_x = normalized_x * depth
|
||||
camera_y = normalized_y * depth
|
||||
camera_z = depth
|
||||
|
||||
# 构建相机坐标向量
|
||||
camera_coordinates = np.array([camera_x, camera_y, camera_z, 1])
|
||||
# print("物体相对相机坐标的齐次坐标: ", camera_coordinates)
|
||||
|
||||
# 将相机坐标转换为机器人底盘坐标
|
||||
robot_coordinates = np.dot(out_matrix, camera_coordinates)[:3]
|
||||
# print("物体的相对底盘坐标为:", robot_coordinates)
|
||||
|
||||
# 将物体相对机器人底盘坐标转为齐次坐标
|
||||
robot_homogeneous_coordinates = np.array([robot_coordinates[0], -robot_coordinates[1], robot_coordinates[2], 1])
|
||||
# print("物体的相对底盘的齐次坐标为:", robot_homogeneous_coordinates)
|
||||
|
||||
# 机器人坐标
|
||||
X = scene.location.X
|
||||
Y = scene.location.Y
|
||||
Z = 0.0
|
||||
|
||||
# 机器人旋转信息
|
||||
Roll = 0.0
|
||||
Pitch = 0.0
|
||||
Yaw = scene.rotation.Yaw
|
||||
|
||||
# 构建平移矩阵
|
||||
T = np.array([[1, 0, 0, X],
|
||||
[0, 1, 0, Y],
|
||||
[0, 0, 1, Z],
|
||||
[0, 0, 0, 1]])
|
||||
|
||||
# 构建旋转矩阵
|
||||
Rx = np.array([[1, 0, 0, 0],
|
||||
[0, np.cos(Roll), -np.sin(Roll), 0],
|
||||
[0, np.sin(Roll), np.cos(Roll), 0],
|
||||
[0, 0, 0, 1]])
|
||||
|
||||
Ry = np.array([[np.cos(Pitch), 0, np.sin(Pitch), 0],
|
||||
[0, 1, 0, 0],
|
||||
[-np.sin(Pitch), 0, np.cos(Pitch), 0],
|
||||
[0, 0, 0, 1]])
|
||||
|
||||
Rz = np.array([[np.cos(np.radians(Yaw)), -np.sin(np.radians(Yaw)), 0, 0],
|
||||
[np.sin(np.radians(Yaw)), np.cos(np.radians(Yaw)), 0, 0],
|
||||
[0, 0, 1, 0],
|
||||
[0, 0, 0, 1]])
|
||||
|
||||
R = np.dot(Rz, np.dot(Ry, Rx))
|
||||
|
||||
# 构建机器人的变换矩阵
|
||||
T_robot = np.dot(T, R)
|
||||
# print(T_robot)
|
||||
|
||||
# 将物体的坐标从机器人底盘坐标系转换到世界坐标系
|
||||
world_coordinates = np.dot(T_robot, robot_homogeneous_coordinates)[:3]
|
||||
|
||||
# if world_coordinates[0] < 200 and world_coordinates[1] <= 1050:
|
||||
# world_coordinates[0] += 400
|
||||
# world_coordinates[1] += 400
|
||||
# elif world_coordinates[0] >= 200 and world_coordinates[1] <= 1050:
|
||||
# world_coordinates[0] -= 550
|
||||
# world_coordinates[1] += 400
|
||||
# elif world_coordinates[0] >= 200 and world_coordinates[1] > 1050:
|
||||
# world_coordinates[0] -= 550
|
||||
# world_coordinates[1] -= 1450
|
||||
# elif world_coordinates[0] < 200 and world_coordinates[1] > 1050:
|
||||
# world_coordinates[0] += 400
|
||||
# world_coordinates[1] -= 1450
|
||||
# print("物体的世界坐标：", world_coordinates)
|
||||
|
||||
# 世界偏移后的坐标
|
||||
world_offest_coordinates = [world_coordinates[0] + 700, world_coordinates[1] + 1400, world_coordinates[2]]
|
||||
# print("物体世界偏移的坐标: ", world_offest_coordinates)
|
||||
return world_coordinates
|
||||
|
||||
@staticmethod
|
||||
def get_obstacle_point(db, scene, map_ratio):
|
||||
# db = DBSCAN(eps=4, min_samples=2)
|
||||
cur_obstacle_pixel_points = []
|
||||
object_pixels = {}
|
||||
colors = [
|
||||
'red',
|
||||
'pink',
|
||||
'purple',
|
||||
'blue',
|
||||
'cyan',
|
||||
'green',
|
||||
'yellow',
|
||||
'orange',
|
||||
'brown',
|
||||
'gold',
|
||||
]
|
||||
not_key_objs_id = {255, 254, 253, 107, 81}
|
||||
|
||||
img_data_segment = get_camera([GrabSim_pb2.CameraName.Head_Segment])
|
||||
img_data_depth = get_camera([GrabSim_pb2.CameraName.Head_Depth])
|
||||
img_data_color = get_camera([GrabSim_pb2.CameraName.Head_Color])
|
||||
|
||||
im_segment = img_data_segment.images[0]
|
||||
im_depth = img_data_depth.images[0]
|
||||
im_color = img_data_color.images[0]
|
||||
|
||||
d_segment = np.frombuffer(im_segment.data, dtype=im_segment.dtype).reshape(
|
||||
(im_segment.height, im_segment.width, im_segment.channels))
|
||||
d_depth = np.frombuffer(im_depth.data, dtype=im_depth.dtype).reshape(
|
||||
(im_depth.height, im_depth.width, im_depth.channels))
|
||||
d_color = np.frombuffer(im_color.data, dtype=im_color.dtype).reshape(
|
||||
(im_color.height, im_color.width, im_color.channels))
|
||||
|
||||
items = img_data_segment.info.split(";")
|
||||
objs_id = {}
|
||||
for item in items:
|
||||
key, value = item.split(":")
|
||||
objs_id[int(key)] = value
|
||||
objs_id[251] = "walker"
|
||||
# plt.imshow(d_depth, cmap="gray" if "depth" in im_depth.name.lower() else None)
|
||||
# plt.show()
|
||||
# plt.imshow(d_segment, cmap="gray" if "depth" in im_segment.name.lower() else None)
|
||||
# plt.axis("off")
|
||||
# plt.title("相机分割")
|
||||
# plt.show()
|
||||
|
||||
d_depth = np.transpose(d_depth, (1, 0, 2))
|
||||
d_segment = np.transpose(d_segment, (1, 0, 2))
|
||||
for i in range(0, d_segment.shape[0], map_ratio):
|
||||
for j in range(0, d_segment.shape[1], map_ratio):
|
||||
if d_depth[i][j][0] == 600:
|
||||
continue
|
||||
|
||||
# if d_segment[i][j] == 96:
|
||||
# print(f"apple的像素坐标：({i},{j})")
|
||||
# print(f"apple的深度：{d_depth[i][j][0]}")
|
||||
# print(f"apple的世界坐标: {transform_co(img_data_depth, i, j, d_depth[i][j][0], scene)}")
|
||||
# if d_segment[i][j] == 113:
|
||||
# print(f"kettle的像素坐标：({i},{j})")
|
||||
# print(f"kettle的深度：{d_depth[i][j][0]}")
|
||||
# print(f"kettle的世界坐标: {transform_co(img_data_depth, i, j, d_depth[i][j][0], scene)}")
|
||||
# if d_segment[i][j][0] in obstacle_objs_id:
|
||||
# cur_obstacle_pixel_points.append([i, j])
|
||||
if d_segment[i][j][0] not in not_key_objs_id:
|
||||
# 首先检查键是否存在
|
||||
if d_segment[i][j][0] in object_pixels:
|
||||
# 如果键存在，那么添加元组(i, j)到对应的值中
|
||||
object_pixels[d_segment[i][j][0]].append([i, j])
|
||||
else:
|
||||
# 如果键不存在，那么创建一个新的键值对，其中键是d_segment[i][j][0]，值是一个包含元组(i, j)的列表
|
||||
object_pixels[d_segment[i][j][0]] = [[i, j]]
|
||||
# print(cur_obstacle_pixel_points)
|
||||
# for pixel in cur_obstacle_pixel_points:
|
||||
# world_point = 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]])
|
||||
# print(f"{pixel}：{[world_point[0], world_point[1]]}")
|
||||
# plt.subplot(2, 2, 2)
|
||||
plt.imshow(d_color, cmap="gray" if "depth" in im_depth.name.lower() else None)
|
||||
# plt.axis('off')
|
||||
plt.title("目标检测")
|
||||
|
||||
|
||||
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)
|
||||
labels = db.labels_
|
||||
# 将数据按照聚类标签分组，并打印每个分组的数据
|
||||
for i in range(max(labels) + 1): # 从0到最大聚类标签的值
|
||||
group_data = X[labels == i] # 获取当前标签的数据
|
||||
x_max = max(p[0] for p in group_data)
|
||||
y_max = max(p[1] for p in group_data)
|
||||
x_min = min(p[0] for p in group_data)
|
||||
y_min = min(p[1] for p in group_data)
|
||||
if x_max - x_min < 10 or y_max - y_min < 10:
|
||||
continue
|
||||
# 在指定的位置绘制方框
|
||||
# 创建矩形框
|
||||
rect = patches.Rectangle((x_min, y_min), (x_max - x_min), (y_max - y_min), linewidth=1,
|
||||
edgecolor=colors[key % 10],
|
||||
facecolor='none')
|
||||
plt.text(x_min, y_min, f'{objs_id[key]}',
|
||||
fontdict={'family': 'serif', 'size': 10, 'color': 'green'}, ha='center',
|
||||
va='center')
|
||||
plt.gca().add_patch(rect)
|
||||
else:
|
||||
x_max = max(p[0] for p in value)
|
||||
y_max = max(p[1] for p in value)
|
||||
x_min = min(p[0] for p in value)
|
||||
y_min = min(p[1] for p in value)
|
||||
# 在指定的位置绘制方框
|
||||
# 创建矩形框
|
||||
rect = patches.Rectangle((x_min, y_min), (x_max - x_min), (y_max - y_min), linewidth=1,
|
||||
edgecolor=colors[key % 10],
|
||||
facecolor='none')
|
||||
|
||||
plt.text(x_min, y_min, f'{objs_id[key]}',
|
||||
fontdict={'family': 'serif', 'size': 10, 'color': 'green'},
|
||||
ha='center',
|
||||
va='center')
|
||||
plt.gca().add_patch(rect)
|
||||
# point1 = min(value, key=lambda x: (x[0], x[1]))
|
||||
# point2 = max(value, key=lambda x: (x[0], x[1]))
|
||||
# width = point2[1] - point1[1]
|
||||
# height = point2[0] - point1[0]
|
||||
# rect = patches.Rectangle((0, 255), 15, 30, linewidth=1, edgecolor='g',
|
||||
# facecolor='none')
|
||||
|
||||
# 将矩形框添加到图像中
|
||||
# plt.gca().add_patch(rect)
|
||||
|
||||
plt.show()
|
||||
# return cur_obstacle_world_points
|
||||
|
|
|
|||
|
|
@ -11,6 +11,8 @@ import pickle
|
|||
|
||||
import numpy as np
|
||||
from matplotlib import pyplot as plt
|
||||
plt.rcParams['font.sans-serif']=['SimHei'] #用来正常显示中文标签
|
||||
plt.rcParams['axes.unicode_minus']=False #用来正常显示负号
|
||||
from sklearn.cluster import DBSCAN
|
||||
|
||||
from robowaiter.scene.scene import Scene
|
||||
|
|
@ -21,21 +23,20 @@ class SceneAEM(Scene):
|
|||
def _reset(self):
|
||||
pass
|
||||
def _run(self):
|
||||
print(len(self.status.objects))
|
||||
# 创建一个从白色（1）到灰色（0）的 colormap
|
||||
objs = self.status.objects
|
||||
cur_objs = []
|
||||
cur_obstacle_world_points = []
|
||||
objs_name_set = set()
|
||||
visited_obstacle = set()
|
||||
obj_json_data = []
|
||||
db = DBSCAN(eps=4, min_samples=2)
|
||||
obj_count = 0
|
||||
added_info = 0
|
||||
|
||||
|
||||
|
||||
map_ratio = 3
|
||||
# # 创建一个颜色映射，其中0表示黑色，1表示白色
|
||||
# cmap = plt.cm.get_cmap('gray')
|
||||
# cmap.set_under('black')
|
||||
# cmap.set_over('white')
|
||||
|
||||
|
||||
db = DBSCAN(eps=map_ratio, min_samples=int(map_ratio / 2))
|
||||
|
||||
file_name = '../../proto/map_1.pkl'
|
||||
if os.path.exists(file_name):
|
||||
|
|
@ -51,11 +52,15 @@ class SceneAEM(Scene):
|
|||
# navigation_test(i,map_id)
|
||||
map_map = np.zeros((math.ceil(950 / map_ratio), math.ceil(1850 / map_ratio)))
|
||||
|
||||
# self.add_walker(0, 30, 520, )
|
||||
# self.add_walker(10, 30, 420)
|
||||
while True:
|
||||
goal = self.explore(map, 120) # cur_pos 指的是当前机器人的位置，场景中应该也有接口可以获取
|
||||
walker_count = 0
|
||||
fig = plt.figure()
|
||||
goal = self.explore(map, 120)
|
||||
if goal is None:
|
||||
break
|
||||
cur_objs, objs_name_set, cur_obstacle_world_points= self.navigation_move(cur_objs, objs_name_set, cur_obstacle_world_points, [[goal[0], goal[1]]], map_ratio, db,0, 11)
|
||||
cur_obstacle_world_points, cur_objs_id= self.navigation_move(plt, cur_objs, cur_obstacle_world_points, [[goal[0], goal[1]]], map_ratio, db,0, 11)
|
||||
|
||||
for point in cur_obstacle_world_points:
|
||||
if point[0] < -350 or point[0] > 600 or point[1] < -400 or point[1] > 1450:
|
||||
|
|
@ -66,34 +71,66 @@ class SceneAEM(Scene):
|
|||
# extent=(-400 / map_ratio, 1450 / map_ratio,
|
||||
# -350 / map_ratio, 600 / map_ratio))
|
||||
|
||||
# 使用imshow函数绘制图像，其中cmap参数设置颜色映射
|
||||
for i in range(len(cur_objs_id)):
|
||||
if cur_objs_id[i] == "walker":
|
||||
walker_count += 1
|
||||
for obj in objs:
|
||||
if obj.name == cur_objs_id[i] and obj not in cur_objs:
|
||||
cur_objs.append(obj)
|
||||
break
|
||||
|
||||
plt.subplot(2, 1, 2) # 这里的2,1表示总共2行，1列，2表示这个位置是第2个子图
|
||||
plt.imshow(map_map, cmap='binary', alpha=0.5, origin='lower',
|
||||
extent=(-400 / map_ratio, 1450 / map_ratio,
|
||||
-350 / map_ratio, 600 / map_ratio))
|
||||
# plt.imshow(map_map, cmap='binary', alpha=0.5, origin='lower')
|
||||
# plt.axis('off')
|
||||
plt.title("地图构建过程")
|
||||
|
||||
plt.subplot(2, 7, 14) # 这里的2,1表示总共2行，1列，2表示这个位置是第2个子图
|
||||
|
||||
# plt.text(0, 0.7, f'检测行人数量：{walker_count}', fontsize=10)
|
||||
|
||||
new_add_info = len(cur_objs) - added_info + walker_count
|
||||
plt.text(0, 0.5, f'新增语义信息：{new_add_info}', fontsize=10) # 在图中添加文字，x和y坐标是在这个图片大小内的相对位置，fontsize是字体大小
|
||||
added_info += new_add_info
|
||||
plt.text(0, 0.3, f'已存语义信息：{added_info}', fontsize=10) # 在图中添加文字，x和y坐标是在这个图片大小内的相对位置，fontsize是字体大小
|
||||
plt.axis("off")
|
||||
plt.show()
|
||||
print("------------当前检测到的物品信息--------------")
|
||||
print(cur_objs)
|
||||
time.sleep(1)
|
||||
print("------------物品信息--------------")
|
||||
print(cur_objs)
|
||||
|
||||
|
||||
for i in range(len(cur_objs)):
|
||||
obj_json_data.append({"id":f"{i}", "name":f"{cur_objs[i].name}", "location":f"{cur_objs[i].location}", "height":f"{cur_objs[i].location.Z * 2}"})
|
||||
if cur_objs[i].name == "Desk" or cur_objs[i].name == "Chair":
|
||||
obj_json_data.append({"id":f"{i}", "name":f"{cur_objs[i].name}", "location":f"{cur_objs[i].location}", "height":f"{cur_objs[i].location.Z * 2}"})
|
||||
|
||||
else:
|
||||
obj_json_data.append(
|
||||
{"id": f"{i}", "name": f"{cur_objs[i].name}", "location": f"{cur_objs[i].location}",
|
||||
"height": f"{cur_objs[i].location.Z}"})
|
||||
|
||||
with open('../../proto/objs.json', 'w') as file:
|
||||
json.dump(obj_json_data, file)
|
||||
|
||||
# for i in range(-350, 600):
|
||||
# for j in range(-400, 1450):
|
||||
# i = (math.floor((i + 350) / map_ratio))
|
||||
# j = (math.floor((j + 400) / map_ratio))
|
||||
# if (i, j) not in visited_obstacle:
|
||||
# map_map[i][j] = 1
|
||||
plt.imshow(map_map, cmap='binary', alpha=0.5, origin='lower',
|
||||
extent=(-400 / map_ratio, 1450 / map_ratio,
|
||||
-350 / map_ratio, 600 / map_ratio))
|
||||
# plt.imshow(map_map, cmap='binary', alpha=0.5, origin='lower',
|
||||
# extent=(-400 / map_ratio, 1450 / map_ratio,
|
||||
# -350 / map_ratio, 600 / map_ratio))
|
||||
# plt.axis('off')
|
||||
plt.show()
|
||||
# plt.show()
|
||||
print("已绘制完成地图！！！")
|
||||
print("------------检测到的所有物品信息--------------")
|
||||
print(obj_json_data)
|
||||
# self.add_walker(0, 30, 520, )
|
||||
# self.add_walker(10, 30, 420)
|
||||
# Scene.get_obstacle_point(db, Scene.status, map_ratio)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
|
|
|||
|
|
@ -0,0 +1,150 @@
|
|||
"""
|
||||
视觉语言操作
|
||||
机器人根据指令人的指令调节空调，自主探索环境导航到目标点，通过手臂的运动规划能力操作空调，比如开关按钮、调温按钮、显示面板
|
||||
"""
|
||||
|
||||
import time
|
||||
from robowaiter.scene.scene import Scene
|
||||
|
||||
|
||||
|
||||
class SceneVLM(Scene):
|
||||
|
||||
def __init__(self, robot):
|
||||
super().__init__(robot)
|
||||
# 在这里加入场景中发生的事件， (事件发生的时间，事件函数)
|
||||
|
||||
self.scene_flag = 1
|
||||
self.st1 = 3
|
||||
# self.st2 = self.st1 + 30
|
||||
# self.st3 = self.st2 + 65
|
||||
self.st2 = 3
|
||||
self.st3 = 3
|
||||
self.st4 = 3
|
||||
|
||||
self.signal_event_list = [
|
||||
|
||||
# 场景1：带小女孩找阳光下的空位
|
||||
(3, self.add_walker, (5, 230, 1200)), # 0号"Girl02_C_3"
|
||||
(1, self.control_walker, (0, False, 200, 60, 520, 0)),
|
||||
(9, self.customer_say, (0, "早上好呀，我想找个能晒太阳的地方。")),
|
||||
(-1, self.customer_say, (0,"可以带我过去嘛？")),
|
||||
(0, self.control_walker, (0, False, 50, 140, 1200, 180)), # 小女孩站在了 BrightTable1 旁边就餐啦
|
||||
|
||||
|
||||
# # 场景2：有个胖胖男人点单交流并要咖啡，帮他送去角落的桌子
|
||||
# (3, self.add_walker, (5, 230, 1200)), # 小女孩
|
||||
# # # 上述准备
|
||||
(10, self.add_walker, (26, -28, -150, 90)),
|
||||
(0, self.add_walker, (10, -70, -200, -45)),
|
||||
(6, self.customer_say, (1, "嘿，RoboWaiter，过来一下！")),
|
||||
(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
|
||||
|
||||
# # 场景3：有位女士要杯水和冰红茶
|
||||
(30 ,self.add_walkers,([[[21, 65, 1000, -90],[32, -80, 850, 135],[1, 60, 420, 135]]])),
|
||||
(0, self.control_walker, (5, True, 50, 250, 1200, 180)), #设置id=4 的2小男孩随机游走红随机游走
|
||||
(0, self.add_walkers, ([[[48, 60, 520,0], [31, 60, 600, -90], [20, 60, 680, -90],[9, 60, 760, -90],[29, -290, 400, 180]]])),
|
||||
|
||||
(5, self.customer_say, (6, "哎呦，今天这么多人，还有空位吗？")),# 女士问
|
||||
(15, self.customer_say, (6, "我带着孩子呢，想要宽敞亮堂的地方。")), # 女士问
|
||||
# 好的，我明白了，那么您可以选择我们的家庭亲子座，这样可以容纳您的孩子，并且更加宽敞舒适。
|
||||
# 这里可以加一下自主导航和探索，找到一个位置
|
||||
# 好的，我明白了，那么我们推荐您到大厅的桌子，那里的空间比较宽敞，环境也比较明亮，适合带着孩子一起用餐。
|
||||
(8, self.customer_say, (6, "大厅的桌子好啊，快带我去呀！")),
|
||||
(15, self.control_walker, (6, False, 50,-250, 480, 0)), # #290, 400
|
||||
(3, self.customer_say, (6, "我想来杯水，帮我孩子拿个酸奶吧。")),
|
||||
# ### 9号灰色男 排队排着排着，不排了
|
||||
(0, self.control_walker, (9, False, 100, 100, 760, 180)),
|
||||
(0, self.control_walker, (9, True, 100, 0, 0, 180)),
|
||||
# # ### 增加场景，孩子说热要开空调 或者9号随机游走和说
|
||||
(90, self.customer_say, (6, "谢谢！不用了。")), #倒水+取放酸奶 90s
|
||||
|
||||
|
||||
|
||||
# # # 场景4：三人排队点单，女士要保温杯
|
||||
(6, self.control_walkers_and_say, ([[[7, False, 100, 60, 520, 180, "我昨天保温杯好像落在你们咖啡厅了，你看到了吗？"]]])),
|
||||
(5, self.customer_say, (7,"你可以帮我拿来吗，我在前门的桌子前等你。")),
|
||||
(1, self.control_walker,(7, False, 80, -10, 520, 90)),# 红女士在吧台前后退一步
|
||||
(1, self.control_walker, (7, False, 80, 240, 1000, -45)), # 红女士走到Table1前
|
||||
(1, self.control_walker, (8, False, 100, 60, 600, -90)), # 大胖男排队往前走一步
|
||||
(2, self.control_walker, (9, False, 100, 60, 680, -90)), # 男灰黑色排队往前走一步
|
||||
(6, self.customer_say, (7,"就是这个杯子！找到啦，好开心！")), # 红女士在Table1前
|
||||
(5, self.customer_say, (7, "不用了。")), # 红女士在Table1前
|
||||
|
||||
|
||||
|
||||
# # 场景5：三人排队点单，一人要冰红茶，一个要点心，一个没座位了赠送保温杯
|
||||
# # 场景6：大胖男点了外卖，离开啦
|
||||
(9, self.control_walkers_and_say, ([[[8, False, 100, 60, 520, 0, "给我来份午餐套餐。"]]])), #原来写了26s
|
||||
(0, self.animation_reset,()), #重置任务
|
||||
(6, self.customer_say, (8, "打包吧，快点！")),
|
||||
(2, self.control_walker, (9, False, 100, 60, 620, -90)), # 男灰黑色排队往前走一步
|
||||
(100, self.customer_say, (8, "谢啦，我赶时间！")), #100这个时间很合适
|
||||
(2, self.control_walker, (8, False, 250, 20, 520, -90)), # 大胖男着急得离开啦
|
||||
(2, self.control_walker, (8, False, 250, 240, -150, -90)),
|
||||
(5, self.remove_walkers, ([[0,7,8]])),
|
||||
|
||||
|
||||
# 场景7：最后排队那个随机游走 9号变为6号，随机游走。
|
||||
# 机器人自主发现任务，走一圈去擦桌子/拖地，碰到灰色男问好，灰色男说“太阳大，要关窗帘和空调调低”
|
||||
# 开了空调
|
||||
(2, self.control_walker, (6, False, 100, 60, 520, 0)), # 10号变7号 男灰黑色排队往前,轮到他
|
||||
(2, self.customer_say, (6, "好热呀！太阳也好大！")),
|
||||
(10, self.control_walkers_and_say, ([[[6, True, 100, 60, 520, 0, "谢谢，这下凉快了"]]])),
|
||||
|
||||
# 场景8 结束了，删除所有顾客。此处增加自主探索发现空间比较暗，打开大厅灯
|
||||
(3, self.clean_walkers, ()),
|
||||
(1, self.add_walker, (17, 60, 1000)),# 增加警察，提醒下班啦
|
||||
(3, self.control_walkers_and_say, ([[[0, False, 150, 60, 520, 0, "下班啦！别忘了打扫卫生。"]]])),
|
||||
(3, self.control_walkers_and_say, ([[[0, False, 150, 60, 520, 0, "不用了。"]]])),
|
||||
|
||||
]
|
||||
|
||||
def _reset(self):
|
||||
self.gen_obj()
|
||||
# self.add_walkers([[47, 920]])
|
||||
pass
|
||||
|
||||
def _run(self, op_type=10):
|
||||
# 一个行人从门口走到 吧台
|
||||
# 打招呼需要什么
|
||||
# 行人说 哪里有位置，想晒个太阳
|
||||
# 带领行人去有太阳的地方
|
||||
# 行人说 有点热
|
||||
# 好的，这就去开空调
|
||||
self.walker_followed = False
|
||||
pass
|
||||
|
||||
def _step(self):
|
||||
|
||||
|
||||
if self.scene_flag == 1:
|
||||
# 如果机器人不在 吧台
|
||||
if self.walker_followed:
|
||||
return
|
||||
end = [self.status.location.X, self.status.location.Y]
|
||||
if end[1]>=600 or end[1]<=450 or end[0]>=250:
|
||||
# if int(self.status.location.X)!=247 or int(self.status.location.X)!=520:
|
||||
self.walker_followed = True
|
||||
self.control_walkers_and_say([[0,False,150,end[0],end[1],90,"谢谢！"]])
|
||||
self.scene_flag += 1
|
||||
|
||||
# 获得所有顾客的名字
|
||||
# print("=================")
|
||||
# for cus in self.status.walkers:
|
||||
# print(cus)
|
||||
# print("=================")
|
||||
pass
|
||||
|
||||
if __name__ == '__main__':
|
||||
import os
|
||||
from robowaiter.robot.robot import Robot
|
||||
|
||||
robot = Robot()
|
||||
|
||||
# create task
|
||||
task = SceneVLM(robot)
|
||||
task.reset()
|
||||
task.run()
|
||||
|
|
@ -34,33 +34,32 @@ class SceneVLM(Scene):
|
|||
|
||||
|
||||
# # 场景2：有个胖胖男人点单交流并要咖啡，帮他送去角落的桌子
|
||||
# (3, self.add_walker, (5, 230, 1200)), # 小女孩
|
||||
(3, self.add_walker, (5, 230, 1200)), # 小女孩
|
||||
# # # 上述准备
|
||||
# (10, self.add_walker, (26, -28, -150, 90)),
|
||||
# (0, self.add_walker, (10, -70, -200, -45)),
|
||||
# (6, self.customer_say, (1, "嘿，RoboWaiter，过来一下！")),
|
||||
# (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, "来杯卡布奇诺吧。")),
|
||||
(10, self.add_walker, (26, -28, -150, 90)),
|
||||
(0, self.add_walker, (10, -70, -200, -45)),
|
||||
(6, self.customer_say, (1, "嘿，RoboWaiter，过来一下！")),
|
||||
(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
|
||||
|
||||
|
||||
# # 场景3：有位女士要杯水和冰红茶
|
||||
# (3, self.add_walker, (5, 230, 1200)),
|
||||
# (30, self.add_walker, (5, 230, 1200)),
|
||||
# (0, self.add_walker, (26, -30, -200, -90)),
|
||||
# (0, self.add_walker, (10, -80, -180, -45)),
|
||||
# # # # 上述准备
|
||||
# # (0, self.add_walker, (21, 65, 1000, -90)), # 男 'BrightTable2': (65, 1000, 135),
|
||||
# # (0, self.add_walker, (32, -80, 850, 135)), # 女 'BrightTable3': (-80, 850, 135),
|
||||
# # (0, self.add_walker, (1, 60, 420, 135)), # 生成小男孩随机游走
|
||||
# (0,self.add_walkers,([[[21, 65, 1000, -90],[32, -80, 850, 135],[1, 60, 420, 135]]])),
|
||||
# (0, self.control_walker, (5, True, 50, 250, 1200, 180)), #设置id=4 的2小男孩随机游走红随机游走
|
||||
# # (0, self.add_walker, (48, 60, 520,0)),# 生成他妈妈
|
||||
# # (0, self.add_walker, (31, 60, 600, -90)), # 女红色排队 7号
|
||||
# # (0, self.add_walker, (20, 60, 680, -90)), # 大胖男色排队 8号
|
||||
# # (0, self.add_walker, (9, 60, 760, -90)), # 男灰黑排队 9号
|
||||
# # (0, self.add_walker, (29, -290, 400, 180)), # 青色女人占了位置 BrightTable5
|
||||
# (0, self.add_walkers, ([[[48, 60, 520,0], [31, 60, 600, -90], [20, 60, 680, -90],[9, 60, 760, -90],[29, -290, 400, 180]]])),
|
||||
# (0, self.add_walker, (21, 65, 1000, -90)), # 男 'BrightTable2': (65, 1000, 135),
|
||||
# (0, self.add_walker, (32, -80, 850, 135)), # 女 'BrightTable3': (-80, 850, 135),
|
||||
# (0, self.add_walker, (1, 60, 420, 135)), # 生成小男孩随机游走
|
||||
(0,self.add_walkers,([[[21, 65, 1000, -90],[32, -80, 850, 135],[1, 60, 420, 135]]])),
|
||||
(0, self.control_walker, (5, True, 50, 250, 1200, 180)), #设置id=4 的2小男孩随机游走红随机游走
|
||||
(0, self.add_walker, (48, 60, 520,0)),# 生成他妈妈
|
||||
# (0, self.add_walker, (31, 60, 600, -90)), # 女红色排队 7号
|
||||
# (0, self.add_walker, (20, 60, 680, -90)), # 大胖男色排队 8号
|
||||
# (0, self.add_walker, (9, 60, 760, -90)), # 男灰黑排队 9号
|
||||
# (0, self.add_walker, (29, -290, 400, 180)), # 青色女人占了位置 BrightTable5
|
||||
(0, self.add_walkers, ([[[48, 60, 520,0], [31, 60, 600, -90], [20, 60, 680, -90],[9, 60, 760, -90],[29, -290, 400, 180]]])),
|
||||
#
|
||||
# # (5, self.customer_say, (6, "哎呦，今天这么多人，还有空位吗？")),# 女士问
|
||||
# # (15, self.customer_say, (6, "我带着孩子呢，想要宽敞亮堂的地方。")), # 女士问
|
||||
|
|
@ -100,47 +99,48 @@ class SceneVLM(Scene):
|
|||
# (1, self.control_walker, (8, False, 100, 60, 600, -90)), # 大胖男排队往前走一步
|
||||
# (2, self.control_walker, (9, False, 100, 60, 680, -90)), # 男灰黑色排队往前走一步
|
||||
# (6, self.customer_say, (7,"就是这个杯子！找到啦，好开心！")), # 红女士在Table1前
|
||||
# (5, self.customer_say, (7, "不用了")), # 红女士在Table1前
|
||||
# (5, self.customer_say, (7, "不用了。")), # 红女士在Table1前
|
||||
|
||||
|
||||
|
||||
# # 场景5：三人排队点单，一人要冰红茶，一个要点心，一个没座位了赠送保温杯
|
||||
(3, self.add_walker, (5, 230, 1200)),
|
||||
(0, self.add_walker, (26, -30, -200, -90)),
|
||||
(0, self.add_walker, (10, -80, -180, -45)),
|
||||
(0, self.add_walker, (21, 65, 1000, -90)), # 男 'BrightTable2': (65, 1000, 135),
|
||||
(0, self.add_walker, (32, -80, 850, 135)),
|
||||
(0, self.add_walker, (1, 60, 220, 135)),
|
||||
(0, self.add_walker, (48, 60, 320, 0)), # 生成他妈妈
|
||||
(0, self.add_walker, (31, 280, 1200, -45)), # # 女红色排队 7号找保温杯的顾客
|
||||
(0, self.add_walker, (20, 60, 680, -90)), # 大胖男排队
|
||||
(0, self.add_walker, (9, 60, 760, -90)), # 男灰黑色排队
|
||||
(0, self.add_walker, (29, -290, 400, 180)), # # 青色女人占了位置 BrightTable5
|
||||
# # 上述准备
|
||||
# # 场景6：大胖男点了外卖，离开啦
|
||||
# (9, self.control_walkers_and_say, ([[[8, False, 100, 60, 520, 0, "给我来份午餐套餐。"]]])), #原来写了26s
|
||||
(0, self.animation_reset,()), #重置任务
|
||||
# (6, self.customer_say, (8, "打包吧，快点！")),
|
||||
# (2, self.control_walker, (9, False, 100, 60, 620, -90)), # 男灰黑色排队往前走一步
|
||||
# # (100, self.customer_say, (8, "谢啦，我赶时间！")), #100这个时间很合适
|
||||
# (2, self.control_walker, (8, False, 250, 20, 520, -90)), # 大胖男着急得离开啦
|
||||
# (2, self.control_walker, (8, False, 250, 240, -150, -90)),
|
||||
(5, self.remove_walkers, ([[0,7,8]])),
|
||||
# (2, self.control_walker, (6, False, 100, 60, 520, 0)), # 9号变7号 男灰黑色排队往前,轮到他
|
||||
|
||||
|
||||
# 场景7：最后排队那个随机游走 9号变为6号，随机游走。
|
||||
# 机器人自主发现任务，走一圈去擦桌子/拖地，碰到灰色男问好，灰色男说“太阳大，要关窗帘和空调调低”
|
||||
# 开了空调
|
||||
(2, self.control_walker, (6, False, 100, 60, 520, 0)), # 10号变7号 男灰黑色排队往前,轮到他
|
||||
# (2, self.customer_say, (6, "好热呀！太阳也好大！")),
|
||||
# (10, self.control_walkers_and_say, ([[[6, True, 100, 60, 520, 0, "谢谢，这下凉快了"]]])),
|
||||
|
||||
|
||||
# 场景8 结束了，删除所有顾客。此处增加自主探索发现空间比较暗，打开大厅灯
|
||||
(3, self.clean_walkers, ()),
|
||||
(1, self.add_walker, (17, 60, 1000)),# 增加警察，提醒下班啦
|
||||
(3, self.control_walkers_and_say, ([[[0, False, 150, 60, 520, 0, "下班啦！别忘了打扫卫生。"]]])),
|
||||
# # # 场景5：三人排队点单，一人要冰红茶，一个要点心，一个没座位了赠送保温杯
|
||||
# (3, self.add_walker, (5, 230, 1200)),
|
||||
# (0, self.add_walker, (26, -30, -200, -90)),
|
||||
# (0, self.add_walker, (10, -80, -180, -45)),
|
||||
# (0, self.add_walker, (21, 65, 1000, -90)), # 男 'BrightTable2': (65, 1000, 135),
|
||||
# (0, self.add_walker, (32, -80, 850, 135)),
|
||||
# (0, self.add_walker, (1, 60, 220, 135)),
|
||||
# (0, self.add_walker, (48, 60, 320, 0)), # 生成他妈妈
|
||||
# (0, self.add_walker, (31, 280, 1200, -45)), # # 女红色排队 7号找保温杯的顾客
|
||||
# (0, self.add_walker, (20, 60, 680, -90)), # 大胖男排队
|
||||
# (0, self.add_walker, (9, 60, 760, -90)), # 男灰黑色排队
|
||||
# (0, self.add_walker, (29, -290, 400, 180)), # # 青色女人占了位置 BrightTable5
|
||||
# # # 上述准备
|
||||
# # # 场景6：大胖男点了外卖，离开啦
|
||||
# # (9, self.control_walkers_and_say, ([[[8, False, 100, 60, 520, 0, "给我来份午餐套餐。"]]])), #原来写了26s
|
||||
# (0, self.animation_reset,()), #重置任务
|
||||
# # (6, self.customer_say, (8, "打包吧，快点！")),
|
||||
# # (2, self.control_walker, (9, False, 100, 60, 620, -90)), # 男灰黑色排队往前走一步
|
||||
# # # (100, self.customer_say, (8, "谢啦，我赶时间！")), #100这个时间很合适
|
||||
# # (2, self.control_walker, (8, False, 250, 20, 520, -90)), # 大胖男着急得离开啦
|
||||
# # (2, self.control_walker, (8, False, 250, 240, -150, -90)),
|
||||
# (5, self.remove_walkers, ([[0,7,8]])),
|
||||
# # (2, self.control_walker, (6, False, 100, 60, 520, 0)), # 9号变7号 男灰黑色排队往前,轮到他
|
||||
#
|
||||
#
|
||||
# # 场景7：最后排队那个随机游走 9号变为6号，随机游走。
|
||||
# # 机器人自主发现任务，走一圈去擦桌子/拖地，碰到灰色男问好，灰色男说“太阳大，要关窗帘和空调调低”
|
||||
# # 开了空调
|
||||
# (2, self.control_walker, (6, False, 100, 60, 520, 0)), # 10号变7号 男灰黑色排队往前,轮到他
|
||||
# # (2, self.customer_say, (6, "好热呀！太阳也好大！")),
|
||||
# # (10, self.control_walkers_and_say, ([[[6, True, 100, 60, 520, 0, "谢谢，这下凉快了"]]])),
|
||||
#
|
||||
#
|
||||
# # 场景8 结束了，删除所有顾客。此处增加自主探索发现空间比较暗，打开大厅灯
|
||||
# (3, self.clean_walkers, ()),
|
||||
# (1, self.add_walker, (17, 60, 1000)),# 增加警察，提醒下班啦
|
||||
# (3, self.control_walkers_and_say, ([[[0, False, 150, 60, 520, 0, "下班啦！别忘了打扫卫生。"]]])),
|
||||
# (3, self.control_walkers_and_say, ([[[0, False, 150, 60, 520, 0, "不用了。"]]])),
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,45 @@
|
|||
"""
|
||||
视觉语言操作
|
||||
机器人根据指令人的指令调节空调，自主探索环境导航到目标点，通过手臂的运动规划能力操作空调，比如开关按钮、调温按钮、显示面板
|
||||
"""
|
||||
|
||||
import time
|
||||
from robowaiter.scene.scene import Scene
|
||||
|
||||
class SceneVLM(Scene):
|
||||
def __init__(self, robot):
|
||||
super().__init__(robot)
|
||||
# 在这里加入场景中发生的事件， (事件发生的时间，事件函数)
|
||||
self.signal_event_list = [
|
||||
(3, self.add_walker, (20,0,700)),
|
||||
(1, self.control_walker, (6, False,100, 60, 520,0)),
|
||||
(1, self.customer_say, (6, "给我来杯酸奶和冰红茶，我坐在对面的桌子那儿。")),
|
||||
(5, self.control_walker, (6, False, 100, -250, 480, 0)),
|
||||
]
|
||||
|
||||
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)
|
||||
|
||||
pass
|
||||
|
||||
def _run(self, op_type=10):
|
||||
|
||||
pass
|
||||
|
||||
def _step(self):
|
||||
pass
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
import os
|
||||
from robowaiter.robot.robot import Robot
|
||||
|
||||
robot = Robot()
|
||||
|
||||
# create task
|
||||
task = SceneVLM(robot)
|
||||
task.reset()
|
||||
task.run()
|
||||
|
|
@ -0,0 +1,43 @@
|
|||
"""
|
||||
视觉语言操作
|
||||
机器人根据指令人的指令调节空调，自主探索环境导航到目标点，通过手臂的运动规划能力操作空调，比如开关按钮、调温按钮、显示面板
|
||||
"""
|
||||
|
||||
import time
|
||||
from robowaiter.scene.scene import Scene
|
||||
|
||||
class SceneVLM(Scene):
|
||||
def __init__(self, robot):
|
||||
super().__init__(robot)
|
||||
# 在这里加入场景中发生的事件， (事件发生的时间，事件函数)
|
||||
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]
|
||||
(1, self.customer_say, (6, "好热呀，想开空调，想要温度调低点！")),
|
||||
(5, self.control_walker, (6, False, 200, 60, 80, 0)),
|
||||
(-1, self.customer_say, (6, "谢谢！这下凉快了！")), #(-100,600)
|
||||
]
|
||||
|
||||
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]],is_autowalk = True)
|
||||
pass
|
||||
|
||||
def _run(self, op_type=10):
|
||||
pass
|
||||
|
||||
def _step(self):
|
||||
pass
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
import os
|
||||
from robowaiter.robot.robot import Robot
|
||||
|
||||
robot = Robot()
|
||||
|
||||
# create task
|
||||
task = SceneVLM(robot)
|
||||
task.reset()
|
||||
task.run()
|
||||
|
|
@ -0,0 +1,142 @@
|
|||
"""
|
||||
视觉语言操作
|
||||
机器人根据指令人的指令调节空调，自主探索环境导航到目标点，通过手臂的运动规划能力操作空调，比如开关按钮、调温按钮、显示面板
|
||||
"""
|
||||
|
||||
import time
|
||||
from robowaiter.scene.scene import Scene
|
||||
|
||||
class SceneVLM(Scene):
|
||||
def __init__(self, robot):
|
||||
super().__init__(robot)
|
||||
# 在这里加入场景中发生的事件， (事件发生的时间，事件函数)
|
||||
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, "好热呀，想开空调，想要温度调低点！")),
|
||||
(5, self.control_walker, (6, False, 200, 60, 80, 0)),
|
||||
(-1, self.customer_say, (6, "谢谢！这下凉快了！")), #(-100,600)
|
||||
|
||||
# 有人提出要开空调和关窗帘
|
||||
# bar (60, 520)
|
||||
# (28, self.add_walker, (0, 0, 0)),
|
||||
# (33, self.control_walker, (7, False, 100, 60, 520, 180)),
|
||||
# (35, self.customer_say, (7,"好热呀！太阳也好大！")),
|
||||
# (45, self.control_walkers_and_say, ([[[7, False, 100, 270, -240, -65, "谢谢，这下凉快了！"]]])),
|
||||
|
||||
# (3, self.add_walker, (0, 0, 0)),
|
||||
# (1, self.control_walker, (5, False, 100, 60, 520, 180)),
|
||||
# (1, self.customer_say, (5,"好热呀！太阳也好大！")),
|
||||
# (-1, self.control_walkers_and_say, ([[[5, False, 100, 270, -240, -65, "谢谢，这下凉快了！"]]])),
|
||||
]
|
||||
|
||||
def _reset(self):
|
||||
self.gen_obj()
|
||||
# self.state["condition_set"] = {'At(Robot,Bar)', 'Is(AC,Off)',
|
||||
# 'Holding(Nothing)','Exist(Yogurt)','Exist(Softdrink)','On(Yogurt,Bar)','On(Softdrink,Table1)',
|
||||
# 'Is(HallLight,Off)', 'Is(TubeLight,On)', 'Is(Curtain,On)',
|
||||
# 'Is(Table1,Dirty)', 'Is(Floor,Dirty)', 'Is(Chairs,Dirty)'}
|
||||
# # 随机生成4个自由行走，一个在 BrightTable4,BrightTable5(-20,220)
|
||||
self.add_walkers([[4,1, 880], [31,250, 1200],[6,-55, 750],[10,70, -200],[27,-290, 400, 180],[26, 60,-320,90]])
|
||||
# [3,1, 880] 1号桌旁边小女孩
|
||||
# [31,250, 1200] 最角落QuietTable1女红色
|
||||
# [6,-55, 750] 1号桌附近小男孩
|
||||
# [10,70, -200] 另一边角落 QuietTable2 男黄色
|
||||
# [27,-290, 400, 180] 中间 BrightTable4 女灰
|
||||
# [26, 60,-320,90] 另一边角落 BrightTable5 红胖男
|
||||
self.control_walkers(walker_loc=[[-55, 750], [70, -200], [250, 1200], [0, 880]],is_autowalk = True)
|
||||
|
||||
# 0-3男孩 4-7女孩 8-26男
|
||||
# 3男孩 31女红 32女灰 10黄色衣服男瘦 9男灰瘦 26红胖男
|
||||
# 17 是员工 police
|
||||
# [0, -150,180]
|
||||
|
||||
|
||||
# self.control_walkers(walker_loc=[[-55, 750]],is_autowalk = False)
|
||||
# 在场景中随机增加一堆行人。
|
||||
# walker_loc = [[-55, 750], [70, -200], [250, 1200], [0, 880]]
|
||||
# controls = []
|
||||
# for i in range(len(s.walkers)):
|
||||
# loc = walker_loc[i]
|
||||
# is_autowalk = False
|
||||
# pose = GrabSim_pb2.Pose(X=loc[0], Y=loc[1], Yaw=180)
|
||||
# controls.append(GrabSim_pb2.WalkerControls.WControl(id=i, autowalk=is_autowalk, speed=200, pose=pose))
|
||||
# scene = sim_client.ControlWalkers(GrabSim_pb2.WalkerControls(controls=controls, scene=scene_id))
|
||||
|
||||
# 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)
|
||||
pass
|
||||
|
||||
def _run(self, op_type=10):
|
||||
# 共17个操作
|
||||
# "制作咖啡","倒水","夹点心","拖地","擦桌子","开筒灯","搬椅子", # 1-7
|
||||
# "关筒灯","开大厅灯","关大厅灯","关闭窗帘","打开窗帘", # 8-12
|
||||
# "调整空调开关","调高空调温度","调低空调温度", # 13-15
|
||||
# "抓握物体","放置物体" # 16-17
|
||||
#
|
||||
# self.gen_obj()
|
||||
# if op_type <=15:
|
||||
# self.move_task_area(op_type)
|
||||
# self.op_task_execute(op_type)
|
||||
# if op_type == 16: # 16: 抓操作需要传入物品id
|
||||
# self.move_task_area(op_type, obj_id=0)
|
||||
# self.op_task_execute(op_type, obj_id=0)
|
||||
# # 原始吧台处:[247.0, 520.0, 100.0], 空调开关旁吧台:[240.0, 40.0, 100.0], 水杯桌:[-70.0, 500.0, 107]
|
||||
# # 桌子1:[-55.0, 0.0, 107],抹布桌:[340.0, 900.0, 99.0] # 桌子2:[-55.0, 150.0, 107],
|
||||
# if op_type == 17: # 17: 放操作需要传入放置位置周围的可达区域
|
||||
# pos = [240.0, 40.0, 100.0]
|
||||
# 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
|
||||
|
||||
def _step(self):
|
||||
pass
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
import os
|
||||
from robowaiter.robot.robot import Robot
|
||||
|
||||
robot = Robot()
|
||||
|
||||
# create task
|
||||
task = SceneVLM(robot)
|
||||
task.reset()
|
||||
task.run()
|
||||
|
|
@ -12,10 +12,11 @@ class SceneVLM(Scene):
|
|||
# 在这里加入场景中发生的事件， (事件发生的时间，事件函数)
|
||||
self.signal_event_list = [
|
||||
(3, self.add_walker, (20,0,700)),
|
||||
(1, self.control_walker, (4, False,100, 60, 520,0)), #[walkerID,autowalk,speed,X,Y,Yaw]
|
||||
(1, self.control_walker, (6, False,100, 60, 520,0)), #[walkerID,autowalk,speed,X,Y,Yaw]
|
||||
# (10, self.customer_say, (6,"给我来杯酸奶和咖啡，哦对，再倒一杯水。")),
|
||||
(-1, self.customer_say, (4, "来杯酸奶吧。")),
|
||||
(-1, self.control_walker, (4, False, 100, -250, 480, 0)), #(-100,600)
|
||||
# (-1, self.customer_say, (6, "来杯酸奶吧。")),
|
||||
(3, self.customer_say, (6, "给我来杯酸奶和冰红茶，我坐在对面的桌子那儿。")),
|
||||
(5, self.control_walker, (6, False, 100, -250, 480, 0)), #(-100,600)
|
||||
|
||||
# 有人提出要开空调和关窗帘
|
||||
# bar (60, 520)
|
||||
|
|
@ -24,19 +25,19 @@ class SceneVLM(Scene):
|
|||
# (35, self.customer_say, (7,"好热呀！太阳也好大！")),
|
||||
# (45, self.control_walkers_and_say, ([[[7, False, 100, 270, -240, -65, "谢谢，这下凉快了！"]]])),
|
||||
|
||||
(3, self.add_walker, (0, 0, 0)),
|
||||
(1, self.control_walker, (5, False, 100, 60, 520, 180)),
|
||||
(1, self.customer_say, (5,"好热呀！太阳也好大！")),
|
||||
(-1, self.control_walkers_and_say, ([[[5, False, 100, 270, -240, -65, "谢谢，这下凉快了！"]]])),
|
||||
# (3, self.add_walker, (0, 0, 0)),
|
||||
# (1, self.control_walker, (5, False, 100, 60, 520, 180)),
|
||||
# (1, self.customer_say, (5,"好热呀！太阳也好大！")),
|
||||
# (-1, self.control_walkers_and_say, ([[[5, False, 100, 270, -240, -65, "谢谢，这下凉快了！"]]])),
|
||||
]
|
||||
|
||||
def _reset(self):
|
||||
self.gen_obj()
|
||||
self.state["condition_set"] = {'At(Robot,Bar)', 'Is(AC,Off)',
|
||||
'Holding(Nothing)','Exist(Yogurt)','Exist(Softdrink)','On(Yogurt,Bar)','On(Softdrink,Table1)',
|
||||
'Is(HallLight,Off)', 'Is(TubeLight,On)', 'Is(Curtain,On)',
|
||||
'Is(Table1,Dirty)', 'Is(Floor,Dirty)', 'Is(Chairs,Dirty)'}
|
||||
# 随机生成4个自由行走，一个在 BrightTable4,BrightTable5(-20,220)
|
||||
# self.state["condition_set"] = {'At(Robot,Bar)', 'Is(AC,Off)',
|
||||
# 'Holding(Nothing)','Exist(Yogurt)','Exist(Softdrink)','On(Yogurt,Bar)','On(Softdrink,Table1)',
|
||||
# 'Is(HallLight,Off)', 'Is(TubeLight,On)', 'Is(Curtain,On)',
|
||||
# 'Is(Table1,Dirty)', 'Is(Floor,Dirty)', 'Is(Chairs,Dirty)'}
|
||||
# # 随机生成4个自由行走，一个在 BrightTable4,BrightTable5(-20,220)
|
||||
self.add_walkers([[4,1, 880], [31,250, 1200],[6,-55, 750],[10,70, -200],[27,-290, 400, 180],[26, 60,-320,90]])
|
||||
# [3,1, 880] 1号桌旁边小女孩
|
||||
# [31,250, 1200] 最角落QuietTable1女红色
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue