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()
|
|||
|
|
|