diff --git a/robowaiter/algos/navigate/test.py b/robowaiter/algos/navigate/test.py index bee148a..ab2b8a0 100644 --- a/robowaiter/algos/navigate/test.py +++ b/robowaiter/algos/navigate/test.py @@ -8,12 +8,13 @@ import matplotlib.pyplot as plt import numpy as np from robowaiter.scene import scene -from navigate import Navigator - +# from navigate import Navigator +from robowaiter.algos.navigate.navigate import Navigator if __name__ == '__main__': +# def navigate_test(scene): file_name = 'map_5.pkl' if os.path.exists(file_name): @@ -37,9 +38,10 @@ if __name__ == '__main__': '''场景3: 移动行人环境 robot到达指定目标''' scene.walk_to(100, 0, -90, dis_limit=10) scene.clean_walker() - scene.add_walker(50, 300, 0) - scene.add_walker(-50, 500, 0) - scene.add_walker(0, 700, 0) + scene.add_walkers([[50, 300],[-50, 500],[0, 700]]) + # scene.add_walker(50, 300, 0) + # scene.add_walker(-50, 500, 0) + # scene.add_walker(0, 700, 0) scene.control_walker([scene.walker_control_generator(walkerID=0, autowalk=False, speed=50, X=-50, Y=600, Yaw=0)]) scene.control_walker([scene.walker_control_generator(walkerID=1, autowalk=False, speed=50, X=100, Y=150, Yaw=0)]) scene.control_walker([scene.walker_control_generator(walkerID=2, autowalk=False, speed=50, X=0, Y=0, Yaw=0)]) @@ -48,8 +50,7 @@ if __name__ == '__main__': # goal = (-300) # goal = (340.0, 900.0) - goal = (240.0, 1000.0) - + # goal = (240.0, 1000.0) # goal = (340.0, 900.0) goal = (240.0, 1160.0) diff --git a/robowaiter/scene/tasks/AEM.py b/robowaiter/scene/tasks/AEM.py index c060341..a6188ad 100644 --- a/robowaiter/scene/tasks/AEM.py +++ b/robowaiter/scene/tasks/AEM.py @@ -15,6 +15,8 @@ class SceneAEM(Scene): def _reset(self): pass def _run(self): + + pass diff --git a/robowaiter/scene/tasks/VLN.py b/robowaiter/scene/tasks/VLN.py index 3a50bcb..448fecf 100644 --- a/robowaiter/scene/tasks/VLN.py +++ b/robowaiter/scene/tasks/VLN.py @@ -16,6 +16,8 @@ from robowaiter.scene.scene import Scene,init_world # TODO: 文件名改成Scen from robowaiter.scene.scene import Scene from robowaiter.utils import get_root_path +from robowaiter.algos.navigate.navigate import Navigator +from robowaiter.algos.navigate import test class SceneVLN(Scene): def __init__(self, robot): @@ -34,6 +36,63 @@ class SceneVLN(Scene): self.state['map']['2d'] = map self.state['map']['obj_pos']['Table'] = np.array((-100, 700)) + def _run(self): + file_name = '../../algos/navigate/map_5.pkl' + if os.path.exists(file_name): + with open(file_name, 'rb') as file: + map = pickle.load(file) + + # self.init_world(1, 11) + # scene = self.status + + navigator = Navigator(scene=self, area_range=[-350, 600, -400, 1450], map=map) + + '''场景1: 无行人环境 robot到达指定目标''' + # goal = np.array((-100, 700)) + + '''场景2: 静止行人环境 robot到达指定目标''' + # scene.clean_walker() + # scene.add_walker(50, 300, 0) + # scene.add_walker(-50, 500, 0) + # goal = np.array((-100, 700)) + + '''场景3: 移动行人环境 robot到达指定目标''' + self.walk_to(100, 0, -90, dis_limit=10) + self.clean_walker() + self.add_walkers([[50, 300], [-50, 500], [0, 700]]) + # scene.add_walker(50, 300, 0) + # scene.add_walker(-50, 500, 0) + # scene.add_walker(0, 700, 0) + self.control_walker( + [self.walker_control_generator(walkerID=0, autowalk=False, speed=50, X=-50, Y=600, Yaw=0)]) + self.control_walker( + [self.walker_control_generator(walkerID=1, autowalk=False, speed=50, X=100, Y=150, Yaw=0)]) + self.control_walker([self.walker_control_generator(walkerID=2, autowalk=False, speed=50, X=0, Y=0, Yaw=0)]) + + # goal = (-100, 700) + # goal = (-300) + # goal = (340.0, 900.0) + + # goal = (240.0, 1000.0) + # goal = (340.0, 900.0) + goal = (240.0, 1160.0) + + '''场景4: 行人自由移动 robot到达指定目标''' + # # TODO: autowalk=True仿真器会闪退 ??? + # scene.clean_walker() + # scene.add_walker(50, 300, 0) + # scene.add_walker(-50, 500, 0) + # scene.control_walker([scene.walker_control_generator(walkerID=0, autowalk=True, speed=20, X=0, Y=0, Yaw=0)]) + # scene.control_walker([scene.walker_control_generator(walkerID=1, autowalk=True, speed=20, X=0, Y=0, Yaw=0)]) + # time.sleep(5) + # goal = np.array((-100, 700)) + + navigator.navigate(goal, animation=False) + + self.clean_walker() + print(self.status.collision) # TODO: 不显示碰撞信息 ??? + pass + if __name__ == '__main__': import os