更新了 VLN

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)
 

robowaiter/scene/tasks/AEM.py

@@ -15,6 +15,8 @@ class SceneAEM(Scene):
     def _reset(self):
         pass
     def _run(self):
+
+
         pass
 
 

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