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

Update scene.py 

解决AEM机器人转向问题
This commit is contained in:
liwang_zhang 2023-11-18 13:59:30 +08:00
parent 51b405fe74
commit d7b5fc9f6c
1 changed files with 25 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
robowaiter/scene/scene.py
View File

@ -6,6 +6,7 @@ import numpy as np
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
from robowaiter.proto import camera from robowaiter.proto import camera
from robowaiter.proto import semantic_map from robowaiter.proto import semantic_map
from robowaiter.algos.navigator.navigate import Navigator
import math import math
from robowaiter.proto import GrabSim_pb2 from robowaiter.proto import GrabSim_pb2
from robowaiter.proto import GrabSim_pb2_grpc from robowaiter.proto import GrabSim_pb2_grpc
@ -668,20 +669,35 @@ class Scene:
def navigation_move(self, cur_objs, objs_name_set, v_list, scene_id=0, map_id=11): def navigation_move(self, cur_objs, objs_name_set, v_list, scene_id=0, map_id=11):
print('------------------navigation_move----------------------') print('------------------navigation_move----------------------')
scene = stub.Observe(GrabSim_pb2.SceneID(value=scene_id)) scene = stub.Observe(GrabSim_pb2.SceneID(value=scene_id))
walk_value = [scene.location.X, scene.location.Y, scene.rotation.Yaw] walk_value = [scene.location.X, scene.location.Y]
print("position:", walk_value) print("position:", walk_value)
if not cur_objs:
walk_v = [scene.location.X, scene.location.Y + 1]
yaw = Navigator.get_yaw(walk_value, walk_v)
walk_v = walk_value + [yaw, 250, 10]
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)
# if scene.info == "Unreachable":
print(scene.info)
# if map_id == 11: # coffee # if map_id == 11: # coffee
# v_list = [[0, 880], [250, 1200], [-55, 750], [70, -200]] # v_list = [[0, 880], [250, 1200], [-55, 750], [70, -200]]
# else: # else:
# v_list = [[0.0, 0.0]] # v_list = [[0.0, 0.0]]
else:
for walk_v in v_list: for walk_v in v_list:
walk_v = walk_v + [scene.rotation.Yaw - 90, 250, 10] yaw = Navigator.get_yaw(walk_value, walk_v)
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 = 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)
# if scene.info == "Unreachable": # if scene.info == "Unreachable":
print(scene.info) print(scene.info)
return cur_objs, objs_name_set return cur_objs, objs_name_set