demo
This commit is contained in:
Antiman-cmyk
parent
590a14ebab
commit
3a4aea4429
|
|
@ -157,4 +157,5 @@ cython_debug/
|
|||
# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
|
||||
# and can be added to the global gitignore or merged into this file. For a more nuclear
|
||||
# option (not recommended) you can uncomment the following to ignore the entire idea folder.
|
||||
#.idea/
|
||||
.idea/
|
||||
test.py
|
||||
|
|
|
|||
|
|
@ -0,0 +1,62 @@
|
|||
#!/usr/bin/env python3
|
||||
# -*- encoding: utf-8 -*-
|
||||
import sys
|
||||
import time
|
||||
import grpc
|
||||
|
||||
sys.path.append('./')
|
||||
sys.path.append('../')
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from mpl_toolkits.axes_grid1 import make_axes_locatable
|
||||
|
||||
from proto import GrabSim_pb2
|
||||
from proto import GrabSim_pb2_grpc
|
||||
|
||||
channel = grpc.insecure_channel('localhost:30001',options=[
|
||||
('grpc.max_send_message_length', 1024*1024*1024),
|
||||
('grpc.max_receive_message_length', 1024*1024*1024)
|
||||
])
|
||||
|
||||
sim_client = GrabSim_pb2_grpc.GrabSimStub(channel)
|
||||
|
||||
def map_test(map_id=0, scene_num=1):
|
||||
initworld = sim_client.Init(GrabSim_pb2.NUL())
|
||||
print(sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL()))
|
||||
initworld = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id))
|
||||
|
||||
def joint_test(scene_id=0):
|
||||
print('------------------joint_test----------------------')
|
||||
action_list = [[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 36.0, -39.37, 37.2, -92.4, 4.13, -0.62, 0.4],
|
||||
[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 36.0, -39.62, 34.75, -94.80, 3.22, -0.26, 0.85],
|
||||
[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 32.63, -32.80, 15.15, -110.70, 6.86, 2.36, 0.40],
|
||||
[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 28.18, -27.92, 6.75, -115.02, 9.46, 4.28, 1.35],
|
||||
[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 4.09, -13.15, -11.97, -107.35, 13.08, 8.58, 3.33]]
|
||||
|
||||
for value in action_list:
|
||||
action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
|
||||
scene = sim_client.Do(action)
|
||||
|
||||
for i in range(8, 21): # arm
|
||||
print(
|
||||
f"{scene.joints[i].name}:{scene.joints[i].angle} location:{scene.joints[i].location.X},{scene.joints[i].location.Y},{scene.joints[i].location.Z}"
|
||||
)
|
||||
print('')
|
||||
for i in range(5, 10): # Right hand
|
||||
print(
|
||||
f"{scene.fingers[i].name} angle:{scene.fingers[i].angle} location:{scene.fingers[i].location[0].X},{scene.fingers[i].location[0].Y},{scene.fingers[i].location[0].Z}"
|
||||
)
|
||||
print('----------------------------------------')
|
||||
time.sleep(0.03)
|
||||
time.sleep(1)
|
||||
|
||||
if __name__ == '__main__':
|
||||
map_id = 3 # 地图编号: 3: 咖啡厅
|
||||
scene_num = 1 # 场景数量
|
||||
map_test(map_id, scene_num) # 场景加载测试
|
||||
time.sleep(5)
|
||||
|
||||
for i in range(scene_num):
|
||||
print("------------------", i, "----------------------")
|
||||
joint_test(i) # 关节控制测试
|
||||
|
|
@ -0,0 +1,97 @@
|
|||
#!/usr/bin/env python3
|
||||
# -*- encoding: utf-8 -*-
|
||||
# enconding = utf8
|
||||
import sys
|
||||
import time
|
||||
import grpc
|
||||
|
||||
sys.path.append('./')
|
||||
sys.path.append('../')
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from mpl_toolkits.axes_grid1 import make_axes_locatable
|
||||
|
||||
from proto import GrabSim_pb2
|
||||
from proto import GrabSim_pb2_grpc
|
||||
|
||||
channel = grpc.insecure_channel('localhost:30001', options=[
|
||||
('grpc.max_send_message_length', 1024 * 1024 * 1024),
|
||||
('grpc.max_receive_message_length', 1024 * 1024 * 1024)
|
||||
])
|
||||
|
||||
sim_client = GrabSim_pb2_grpc.GrabSimStub(channel)
|
||||
|
||||
|
||||
def map_test(map_id=0, scene_num=1):
|
||||
initworld = sim_client.Init(GrabSim_pb2.NUL())
|
||||
print(sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL()))
|
||||
initworld = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id))
|
||||
|
||||
|
||||
def control_robot_action(scene_id=0, type=0, action=0, message="你好"):
|
||||
scene = sim_client.ControlRobot(GrabSim_pb2.ControlInfo(scene=scene_id, type=type, action=action, content=message))
|
||||
if (scene.info == "action success"):
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
map_id = 3 # 地图编号: 0：空房间 1：室内 2:咖啡厅1.0 3: 咖啡厅2.0 4:餐厅 5:养老院 6：会议室
|
||||
scene_num = 1 # 场景数量
|
||||
map_test(map_id, scene_num) # 场景加载测试
|
||||
time.sleep(5)
|
||||
|
||||
# 制作咖啡
|
||||
control_robot_action(0, 0, 1, "开始制作咖啡")
|
||||
result = control_robot_action(0, 1, 1)
|
||||
if (result):
|
||||
control_robot_action(0, 1, 2)
|
||||
control_robot_action(0, 1, 3)
|
||||
control_robot_action(0, 1, 4)
|
||||
else:
|
||||
control_robot_action(0, 0, 1, "制作咖啡失败")
|
||||
|
||||
# 倒水
|
||||
control_robot_action(0, 0, 1, "开始倒水")
|
||||
result = control_robot_action(0, 2, 1)
|
||||
if (result):
|
||||
control_robot_action(0, 2, 2)
|
||||
control_robot_action(0, 2, 3)
|
||||
control_robot_action(0, 2, 4)
|
||||
control_robot_action(0, 2, 5)
|
||||
else:
|
||||
control_robot_action(0, 0, 1, "倒水失败")
|
||||
|
||||
# 夹点心
|
||||
control_robot_action(0, 0, 1, "开始夹点心")
|
||||
result = control_robot_action(0, 3, 1)
|
||||
if (result):
|
||||
control_robot_action(0, 3, 2)
|
||||
control_robot_action(0, 3, 3)
|
||||
control_robot_action(0, 3, 4)
|
||||
control_robot_action(0, 3, 5)
|
||||
control_robot_action(0, 3, 6)
|
||||
control_robot_action(0, 3, 7)
|
||||
else:
|
||||
control_robot_action(0, 0, 1, "夹点心失败")
|
||||
|
||||
# 拖地
|
||||
control_robot_action(0, 0, 1, "开始拖地")
|
||||
result = control_robot_action(0, 4, 1)
|
||||
if (result):
|
||||
control_robot_action(0, 4, 2)
|
||||
control_robot_action(0, 4, 3)
|
||||
control_robot_action(0, 4, 4)
|
||||
else:
|
||||
control_robot_action(0, 0, 1, "拖地失败")
|
||||
|
||||
# 擦桌子
|
||||
control_robot_action(0, 0, 1, "开始擦桌子")
|
||||
result = control_robot_action(0, 5, 1)
|
||||
if (result):
|
||||
control_robot_action(0, 5, 2)
|
||||
control_robot_action(0, 5, 3)
|
||||
else:
|
||||
control_robot_action(0, 0, 1, "擦桌子失败")
|
||||
|
|
@ -0,0 +1,50 @@
|
|||
#!/usr/bin/env python3
|
||||
# -*- encoding: utf-8 -*-
|
||||
import sys
|
||||
import time
|
||||
import grpc
|
||||
|
||||
sys.path.append('./')
|
||||
sys.path.append('../')
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from mpl_toolkits.axes_grid1 import make_axes_locatable
|
||||
|
||||
from proto import GrabSim_pb2
|
||||
from proto import GrabSim_pb2_grpc
|
||||
|
||||
channel = grpc.insecure_channel('localhost:30001',options=[
|
||||
('grpc.max_send_message_length', 1024*1024*1024),
|
||||
('grpc.max_receive_message_length', 1024*1024*1024)
|
||||
])
|
||||
|
||||
sim_client = GrabSim_pb2_grpc.GrabSimStub(channel)
|
||||
|
||||
def map_test(map_id=0, scene_num=1):
|
||||
initworld = sim_client.Init(GrabSim_pb2.NUL())
|
||||
print(sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL()))
|
||||
initworld = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id))
|
||||
|
||||
def reset(scene_id=0):
|
||||
scene = sim_client.Reset(GrabSim_pb2.ResetParams(scene=scene_id))
|
||||
|
||||
def show_env_info(scene_id=0):
|
||||
scene = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id))
|
||||
print('------------------show_env_info----------------------')
|
||||
print(
|
||||
f"location:{[scene.location.X, scene.location.Y]}, rotation:{scene.rotation.Yaw}\n",
|
||||
f"joints number:{len(scene.joints)}, fingers number:{len(scene.fingers)}\n", f"objects number: {len(scene.objects)}\n"
|
||||
f"rotation:{scene.rotation}, timestep:{scene.timestep}\n"
|
||||
f"timestamp:{scene.timestamp}, collision:{scene.collision}, info:{scene.info}")
|
||||
|
||||
if __name__ == '__main__':
|
||||
map_id = 3 # 地图编号: 3: 咖啡厅
|
||||
scene_num = 1 # 场景数量
|
||||
map_test(map_id, scene_num) # 场景加载测试
|
||||
time.sleep(5)
|
||||
|
||||
for i in range(scene_num):
|
||||
print("------------------", i, "----------------------")
|
||||
reset(i) # 场景重置测试
|
||||
show_env_info(i) # 场景信息测试
|
||||
|
|
@ -0,0 +1,63 @@
|
|||
#!/usr/bin/env python3
|
||||
# -*- encoding: utf-8 -*-
|
||||
import sys
|
||||
import time
|
||||
import grpc
|
||||
|
||||
sys.path.append('./')
|
||||
sys.path.append('../')
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from mpl_toolkits.axes_grid1 import make_axes_locatable
|
||||
|
||||
from proto import GrabSim_pb2
|
||||
from proto import GrabSim_pb2_grpc
|
||||
|
||||
channel = grpc.insecure_channel('localhost:30001',options=[
|
||||
('grpc.max_send_message_length', 1024*1024*1024),
|
||||
('grpc.max_receive_message_length', 1024*1024*1024)
|
||||
])
|
||||
|
||||
sim_client = GrabSim_pb2_grpc.GrabSimStub(channel)
|
||||
|
||||
def map_test(map_id=0, scene_num=1):
|
||||
initworld = sim_client.Init(GrabSim_pb2.NUL())
|
||||
print(sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL()))
|
||||
initworld = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id))
|
||||
|
||||
def walk_test(scene_id=0, map_id=0):
|
||||
"""
|
||||
移动底盘
|
||||
GrabSim_pb2.Action(sceneID=0, action=GrabSim_pb2.Action.ActionType.WalkTo, values=[x, y, yaw, velocity, dis])
|
||||
yaw: 机器人朝向; velocity:速度, -1代表瞬移; dis:最终达到的位置距离目标点最远距离, 如果超过此距离则目标位置不可达
|
||||
"""
|
||||
scene = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id))
|
||||
|
||||
walk_value = [scene.location.X, scene.location.Y, scene.rotation.Yaw]
|
||||
print('------------------walk_test----------------------')
|
||||
print("position:", walk_value)
|
||||
|
||||
if map_id == 3: # coffee
|
||||
v_list = [[scene.location.X + 20, scene.location.Y - 500], [scene.location.X - 200, scene.location.Y - 300],
|
||||
[scene.location.X - 200, scene.location.Y + 20], [0, 880], [250, 1200], [-55, 750], [70, -200]]
|
||||
else:
|
||||
v_list = [[scene.location.X - 10, scene.location.Y - 20]]
|
||||
|
||||
for walk_v in v_list:
|
||||
walk_v = walk_v + [scene.rotation.Yaw - 90, 600, 100]
|
||||
print("walk_v", walk_v)
|
||||
action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v)
|
||||
scene = sim_client.Do(action)
|
||||
print(scene.info) # print navigation info
|
||||
time.sleep(2)
|
||||
|
||||
if __name__ == '__main__':
|
||||
map_id = 3 # 地图编号: 3: 咖啡厅
|
||||
scene_num = 1 # 场景数量
|
||||
map_test(map_id, scene_num) # 场景加载测试
|
||||
time.sleep(5)
|
||||
|
||||
for i in range(scene_num):
|
||||
print("------------------", i, "----------------------")
|
||||
walk_test(i, map_id) # 导航寻路测试
|
||||
|
|
@ -0,0 +1,47 @@
|
|||
#!/usr/bin/env python3
|
||||
# -*- encoding: utf-8 -*-
|
||||
# enconding = utf8
|
||||
import sys
|
||||
import time
|
||||
import grpc
|
||||
|
||||
sys.path.append('./')
|
||||
sys.path.append('../')
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from mpl_toolkits.axes_grid1 import make_axes_locatable
|
||||
|
||||
from proto import GrabSim_pb2
|
||||
from proto import GrabSim_pb2_grpc
|
||||
|
||||
channel = grpc.insecure_channel('localhost:30001', options=[
|
||||
('grpc.max_send_message_length', 1024 * 1024 * 1024),
|
||||
('grpc.max_receive_message_length', 1024 * 1024 * 1024)
|
||||
])
|
||||
|
||||
sim_client = GrabSim_pb2_grpc.GrabSimStub(channel)
|
||||
|
||||
|
||||
def map_test(map_id=0, scene_num=1):
|
||||
initworld = sim_client.Init(GrabSim_pb2.NUL())
|
||||
print(sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL()))
|
||||
initworld = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id))
|
||||
|
||||
|
||||
def control_robot_action(scene_id=0, type=0, action=0, message="你好"):
|
||||
scene = sim_client.ControlRobot(GrabSim_pb2.ControlInfo(scene=scene_id, type=type, action=action, content=message))
|
||||
if (scene.info == "action success"):
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
map_id = 3 # 地图编号: 0：空房间 1：室内 2:咖啡厅1.0 3: 咖啡厅2.0 4:餐厅 5:养老院 6：会议室
|
||||
scene_num = 1 # 场景数量
|
||||
map_test(map_id, scene_num) # 场景加载测试
|
||||
time.sleep(5)
|
||||
|
||||
# 文字冒泡
|
||||
control_robot_action(0, 0, 1, "你好，欢迎光临")
|
||||
|
|
@ -0,0 +1,106 @@
|
|||
#!/usr/bin/env python3
|
||||
# -*- encoding: utf-8 -*-
|
||||
import sys
|
||||
import time
|
||||
import grpc
|
||||
|
||||
sys.path.append('./')
|
||||
sys.path.append('../')
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from mpl_toolkits.axes_grid1 import make_axes_locatable
|
||||
|
||||
from proto import GrabSim_pb2
|
||||
from proto import GrabSim_pb2_grpc
|
||||
|
||||
channel = grpc.insecure_channel('localhost:30001',options=[
|
||||
('grpc.max_send_message_length', 1024*1024*1024),
|
||||
('grpc.max_receive_message_length', 1024*1024*1024)
|
||||
])
|
||||
|
||||
sim_client = GrabSim_pb2_grpc.GrabSimStub(channel)
|
||||
|
||||
def map_test(map_id=0, scene_num=1):
|
||||
initworld = sim_client.Init(GrabSim_pb2.NUL())
|
||||
print(sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL()))
|
||||
initworld = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id))
|
||||
|
||||
def joint_test(scene_id=0):
|
||||
print('------------------joint_test----------------------')
|
||||
action_list = [[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 36.0, -39.37, 37.2, -92.4, 4.13, -0.62, 0.4],
|
||||
[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 36.0, -39.62, 34.75, -94.80, 3.22, -0.26, 0.85],
|
||||
[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 32.63, -32.80, 15.15, -110.70, 6.86, 2.36, 0.40],
|
||||
[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 28.18, -27.92, 6.75, -115.02, 9.46, 4.28, 1.35],
|
||||
[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 4.09, -13.15, -11.97, -107.35, 13.08, 8.58, 3.33]]
|
||||
|
||||
for value in action_list:
|
||||
action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
|
||||
scene = sim_client.Do(action)
|
||||
|
||||
for i in range(8, 21): # arm
|
||||
print(
|
||||
f"{scene.joints[i].name}:{scene.joints[i].angle} location:{scene.joints[i].location.X},{scene.joints[i].location.Y},{scene.joints[i].location.Z}"
|
||||
)
|
||||
print('')
|
||||
for i in range(5, 10): # Right hand
|
||||
print(
|
||||
f"{scene.fingers[i].name} angle:{scene.fingers[i].angle} location:{scene.fingers[i].location[0].X},{scene.fingers[i].location[0].Y},{scene.fingers[i].location[0].Z}"
|
||||
)
|
||||
print('----------------------------------------')
|
||||
time.sleep(0.03)
|
||||
time.sleep(1)
|
||||
|
||||
def gen_obj(scene_id, h=80):
|
||||
print('------------------gen objs----------------------')
|
||||
scene = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id))
|
||||
ginger_loc = [scene.location.X, scene.location.Y, scene.location.Z]
|
||||
|
||||
obj_list = [
|
||||
GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 90, y=ginger_loc[1] + 30, yaw=10, z=h, type=4),
|
||||
GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 80, y=ginger_loc[1] + 31, z=h, type=5),
|
||||
GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 33, y=ginger_loc[1] - 10.5, z=h+20, type=7),
|
||||
GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 70, y=ginger_loc[1] + 33, z=h, type=9),
|
||||
GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 60, y=ginger_loc[1] + 34, z=h, type=13)
|
||||
]
|
||||
scene = sim_client.AddObjects(GrabSim_pb2.ObjectList(objects=obj_list, scene=scene_id))
|
||||
print(scene.collision)
|
||||
time.sleep(5)
|
||||
|
||||
def remove_obj(scene_id=0, id_list=[1]):
|
||||
print('------------------remove objs----------------------')
|
||||
remove_obj_list = id_list
|
||||
scene = sim_client.RemoveObjects(GrabSim_pb2.RemoveList(IDs=remove_obj_list, scene=scene_id))
|
||||
print(f"remove objects {id_list}. current obj:")
|
||||
time.sleep(1)
|
||||
|
||||
def clean_obj(scene_id=0):
|
||||
print('------------------clean objs----------------------')
|
||||
scene = sim_client.CleanObjects(GrabSim_pb2.SceneID(value=scene_id))
|
||||
|
||||
def obj_test(scene_id=0):
|
||||
gen_obj(scene_id)
|
||||
# remove_obj(scene_id, id_list=[0])
|
||||
# clean_obj(scene_id)
|
||||
|
||||
def grasp_test(hand_id, obj_scene_id, scene_id=0):
|
||||
action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.Grasp, values=[hand_id, obj_scene_id])
|
||||
scene = sim_client.Do(action)
|
||||
|
||||
def release_test(hand_id, scene_id=0):
|
||||
action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.Release, values=[hand_id])
|
||||
scene = sim_client.Do(action)
|
||||
|
||||
if __name__ == '__main__':
|
||||
map_id = 3 # 地图编号: 3: 咖啡厅
|
||||
scene_num = 1 # 场景数量
|
||||
map_test(map_id, scene_num) # 场景加载测试
|
||||
time.sleep(5)
|
||||
|
||||
for i in range(scene_num):
|
||||
print("------------------", i, "----------------------")
|
||||
joint_test(i) # 关节控制测试
|
||||
obj_test(i) # 物品生成测试
|
||||
grasp_test(1, 2) # 抓取物品测试
|
||||
joint_test(i) # 关节控制测试
|
||||
release_test(1) # 释放物品测试
|
||||
|
|
@ -0,0 +1,52 @@
|
|||
#!/usr/bin/env python3
|
||||
# -*- encoding: utf-8 -*-
|
||||
import sys
|
||||
import time
|
||||
import grpc
|
||||
|
||||
sys.path.append('./')
|
||||
sys.path.append('../')
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from mpl_toolkits.axes_grid1 import make_axes_locatable
|
||||
|
||||
from proto import GrabSim_pb2
|
||||
from proto import GrabSim_pb2_grpc
|
||||
|
||||
channel = grpc.insecure_channel('localhost:30001',options=[
|
||||
('grpc.max_send_message_length', 1024*1024*1024),
|
||||
('grpc.max_receive_message_length', 1024*1024*1024)
|
||||
])
|
||||
|
||||
sim_client = GrabSim_pb2_grpc.GrabSimStub(channel)
|
||||
|
||||
def map_test(map_id=0, scene_num=1):
|
||||
initworld = sim_client.Init(GrabSim_pb2.NUL())
|
||||
print(sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL()))
|
||||
initworld = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id))
|
||||
|
||||
def get_camera(part, scene_id=0):
|
||||
action = GrabSim_pb2.CameraList(cameras=part, scene=scene_id)
|
||||
return sim_client.Capture(action)
|
||||
|
||||
def show_image(img_data):
|
||||
im = img_data.images[0]
|
||||
d = np.frombuffer(im.data, dtype=im.dtype).reshape((im.height, im.width, im.channels))
|
||||
plt.imshow(d, cmap="gray" if "depth" in im.name.lower() else None)
|
||||
plt.show()
|
||||
|
||||
def camera_test(scene_id=0):
|
||||
for camera_name in [GrabSim_pb2.CameraName.Head_Color, GrabSim_pb2.CameraName.Head_Depth, GrabSim_pb2.CameraName.Head_Segment]:
|
||||
img_data = get_camera([camera_name], scene_id)
|
||||
show_image(img_data)
|
||||
|
||||
if __name__ == '__main__':
|
||||
map_id = 3 # 地图编号: 0：空房间 1：室内 2:咖啡厅1.0 3: 咖啡厅2.0 4:餐厅 5:养老院 6：会议室
|
||||
scene_num = 1 # 场景数量
|
||||
map_test(map_id, scene_num) # 场景加载测试
|
||||
time.sleep(5)
|
||||
|
||||
for i in range(scene_num):
|
||||
print("------------------", i, "----------------------")
|
||||
camera_test(i) # 相机操作测试
|
||||
|
|
@ -0,0 +1,87 @@
|
|||
#!/usr/bin/env python3
|
||||
# -*- encoding: utf-8 -*-
|
||||
import sys
|
||||
import time
|
||||
import grpc
|
||||
|
||||
sys.path.append('./')
|
||||
sys.path.append('../')
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from mpl_toolkits.axes_grid1 import make_axes_locatable
|
||||
|
||||
from proto import GrabSim_pb2
|
||||
from proto import GrabSim_pb2_grpc
|
||||
|
||||
channel = grpc.insecure_channel('localhost:30001',options=[
|
||||
('grpc.max_send_message_length', 1024*1024*1024),
|
||||
('grpc.max_receive_message_length', 1024*1024*1024)
|
||||
])
|
||||
|
||||
sim_client = GrabSim_pb2_grpc.GrabSimStub(channel)
|
||||
|
||||
def map_test(map_id=0, scene_num=1):
|
||||
initworld = sim_client.Init(GrabSim_pb2.NUL())
|
||||
print(sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL()))
|
||||
initworld = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id))
|
||||
|
||||
def add_walker(scene_id=0):
|
||||
"""pose:表示行人的初始位置姿态"""
|
||||
print('------------------add walker----------------------')
|
||||
s = sim_client.Observe(GrabSim_pb2.SceneID(value=0))
|
||||
|
||||
walker_loc = [[0, 880], [250, 1200], [-55, 750], [70, -200]]
|
||||
walker_list = []
|
||||
for i in range(len(walker_loc)):
|
||||
loc = walker_loc[i] + [0, 600, 100]
|
||||
action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=loc)
|
||||
scene = sim_client.Do(action)
|
||||
print(scene.info)
|
||||
# 只有可达的位置才能成功初始化行人，显示unreachable的地方不能初始化行人
|
||||
walker_list.append(GrabSim_pb2.WalkerList.Walker(id=i, pose=GrabSim_pb2.Pose(X=loc[0], Y=loc[1], Yaw=90)))
|
||||
|
||||
scene = sim_client.AddWalker(GrabSim_pb2.WalkerList(walkers=walker_list, scene=scene_id))
|
||||
return scene
|
||||
|
||||
def control_walkers(scene_id=0):
|
||||
"""pose:表示行人的终止位置姿态"""
|
||||
s = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id))
|
||||
|
||||
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))
|
||||
time.sleep(10)
|
||||
return scene
|
||||
|
||||
def remove_walkers(scene_id=0):
|
||||
s = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id))
|
||||
scene = sim_client.RemoveWalkers(GrabSim_pb2.RemoveList(IDs=[1, 3], scene=scene_id))
|
||||
time.sleep(2)
|
||||
return
|
||||
|
||||
def clean_walkers(scene_id=0):
|
||||
scene = sim_client.CleanWalkers(GrabSim_pb2.SceneID(value=scene_id))
|
||||
return scene
|
||||
|
||||
def walker_test(scene_id=0):
|
||||
add_walker(scene_id)
|
||||
control_walkers(scene_id)
|
||||
remove_walkers(scene_id)
|
||||
clean_walkers(scene_id)
|
||||
return
|
||||
|
||||
if __name__ == '__main__':
|
||||
map_id = 3 # 地图编号: 3: 咖啡厅
|
||||
scene_num = 1 # 场景数量
|
||||
map_test(map_id, scene_num) # 场景加载测试
|
||||
time.sleep(5)
|
||||
|
||||
for i in range(scene_num):
|
||||
print("------------------", i, "----------------------")
|
||||
walker_test(i) # 行人控制测试
|
||||
Loading…
Reference in New Issue