87 lines
2.2 KiB
Python
87 lines
2.2 KiB
Python
"""
|
||
视觉语言导航
|
||
|
||
测试导航算法的动态避障(Obstacle Detection and Avoidance, ODA)能力
|
||
|
||
|
||
|
||
"""
|
||
import os
|
||
import pickle
|
||
import time
|
||
import random
|
||
|
||
import matplotlib.pyplot as plt
|
||
import numpy as np
|
||
|
||
from robowaiter.scene.scene import Scene
|
||
from robowaiter.utils import get_root_path
|
||
|
||
|
||
|
||
class SceneVLN(Scene):
|
||
def __init__(self, robot):
|
||
super().__init__(robot)
|
||
# 在这里加入场景中发生的事件， (事件发生的时间，事件函数)
|
||
# self.event_list = [
|
||
# (5, self.create_chat_event("测试VLN：前往水台")),
|
||
# ]
|
||
|
||
def _reset(self):
|
||
'''
|
||
设置场景 (添加行人...)
|
||
'''
|
||
# root_path = get_root_path()
|
||
# map_file = os.path.join(root_path, 'robowaiter/algos/navigator/map_4.pkl')
|
||
# with open(map_file, 'rb') as file:
|
||
# map = pickle.load(file)
|
||
# self.state['map']['2d'] = map
|
||
|
||
|
||
# 角1:[480, 1300, 90]
|
||
# 窗1:[230, 1200, 135]
|
||
# 窗2:[65, 1000, 135.0]
|
||
# 窗3:[-80, 850, 135]
|
||
# 窗4:[-270, 520, 150]
|
||
# 窗5:[-20, -220, -100]
|
||
# 角2:[250, -240, -65]
|
||
|
||
|
||
# 1. robot位置初始化
|
||
self.walk_to(20, 20, velocity=150, dis_limit=10)
|
||
|
||
# 2. 添加行人
|
||
self.clean_walker()
|
||
self.add_walkers([[-60, 700], [-20, 680], [20, 300], [60, 300]])
|
||
|
||
# walker_0:
|
||
self.control_walker([self.walker_control_generator(walkerID=0, autowalk=False, speed=30, X=-60, Y=0, Yaw=0)])
|
||
|
||
# walker_1:
|
||
self.control_walker([self.walker_control_generator(walkerID=1, autowalk=False, speed=30, X=-20, Y=0, Yaw=0)])
|
||
|
||
# walker_2:
|
||
self.control_walker([self.walker_control_generator(walkerID=2, autowalk=False, speed=30, X=20, Y=0, Yaw=0)])
|
||
|
||
# walker_3:
|
||
self.control_walker([self.walker_control_generator(walkerID=3, autowalk=False, speed=30, X=60, Y=0, Yaw=0)])
|
||
|
||
# 3. 导航
|
||
goal = (230, 1200)
|
||
self.navigator.navigate(goal=goal, animation=False)
|
||
|
||
# self.walk_to(goal[0], goal[1], velocity=150, dis_limit=10)
|
||
|
||
|
||
if __name__ == '__main__':
|
||
import os
|
||
from robowaiter.robot.robot import Robot
|
||
|
||
robot = Robot()
|
||
|
||
# create task
|
||
task = SceneVLN(robot)
|
||
task.reset()
|
||
task.run()
|
||
|