257 lines
7.3 KiB
Python
257 lines
7.3 KiB
Python
"""
|
|
演示场景工具
|
|
提供各种路径规划演示场景的创建功能
|
|
"""
|
|
|
|
from typing import List, Tuple
|
|
import random
|
|
|
|
def create_simple_grid(rows: int, cols: int) -> List[List[int]]:
|
|
"""
|
|
创建简单的空网格
|
|
|
|
Args:
|
|
rows: 行数
|
|
cols: 列数
|
|
|
|
Returns:
|
|
网格数据
|
|
"""
|
|
return [[0 for _ in range(cols)] for _ in range(rows)]
|
|
|
|
def create_maze_grid(rows: int, cols: int) -> List[List[int]]:
|
|
"""
|
|
创建迷宫网格（使用深度优先搜索算法生成）
|
|
|
|
Args:
|
|
rows: 行数
|
|
cols: 列数
|
|
|
|
Returns:
|
|
迷宫网格数据
|
|
"""
|
|
# 确保行列数为奇数
|
|
if rows % 2 == 0:
|
|
rows += 1
|
|
if cols % 2 == 0:
|
|
cols += 1
|
|
|
|
# 初始化网格（全部设为墙）
|
|
grid = [[1 for _ in range(cols)] for _ in range(rows)]
|
|
|
|
# 使用深度优先搜索生成迷宫
|
|
stack = []
|
|
|
|
# 起点
|
|
start_row, start_col = 1, 1
|
|
grid[start_row][start_col] = 0
|
|
stack.append((start_row, start_col))
|
|
|
|
# 方向：上、右、下、左
|
|
directions = [(-2, 0), (0, 2), (2, 0), (0, -2)]
|
|
|
|
while stack:
|
|
current_row, current_col = stack[-1]
|
|
|
|
# 获取未访问的邻居
|
|
neighbors = []
|
|
for dr, dc in directions:
|
|
new_row, new_col = current_row + dr, current_col + dc
|
|
if 1 <= new_row < rows - 1 and 1 <= new_col < cols - 1:
|
|
if grid[new_row][new_col] == 1:
|
|
neighbors.append((new_row, new_col, dr // 2, dc // 2))
|
|
|
|
if neighbors:
|
|
# 随机选择一个邻居
|
|
new_row, new_col, wall_row, wall_col = random.choice(neighbors)
|
|
|
|
# 打通当前单元格和选中邻居之间的墙
|
|
grid[new_row][new_col] = 0
|
|
grid[current_row + wall_row][current_col + wall_col] = 0
|
|
|
|
# 将选中的邻居加入栈
|
|
stack.append((new_row, new_col))
|
|
else:
|
|
# 回溯
|
|
stack.pop()
|
|
|
|
return grid
|
|
|
|
def create_room_grid(rows: int, cols: int) -> List[List[int]]:
|
|
"""
|
|
创建房间网格（包含多个房间和走廊）
|
|
|
|
Args:
|
|
rows: 行数
|
|
cols: 列数
|
|
|
|
Returns:
|
|
房间网格数据
|
|
"""
|
|
grid = [[0 for _ in range(cols)] for _ in range(rows)]
|
|
|
|
# 添加一些房间（用障碍物围起来的空心矩形）
|
|
rooms = [
|
|
(2, 2, 5, 5), # (row, col, height, width)
|
|
(2, 10, 5, 5),
|
|
(10, 2, 5, 5),
|
|
(10, 10, 5, 5)
|
|
]
|
|
|
|
for room_row, room_col, room_height, room_width in rooms:
|
|
# 创建房间的墙
|
|
for r in range(room_row, min(room_row + room_height, rows)):
|
|
for c in range(room_col, min(room_col + room_width, cols)):
|
|
if (r == room_row or r == room_row + room_height - 1 or
|
|
c == room_col or c == room_col + room_width - 1):
|
|
if 0 <= r < rows and 0 <= c < cols:
|
|
grid[r][c] = 1
|
|
|
|
# 添加走廊连接房间
|
|
# 水平走廊
|
|
for c in range(7, 10):
|
|
if 4 < rows:
|
|
grid[4][c] = 0
|
|
|
|
# 垂直走廊
|
|
for r in range(7, 10):
|
|
if 4 < cols:
|
|
grid[r][4] = 0
|
|
if 12 < cols:
|
|
grid[r][12] = 0
|
|
|
|
return grid
|
|
|
|
def create_random_grid(rows: int, cols: int, obstacle_probability: float = 0.3) -> List[List[int]]:
|
|
"""
|
|
创建随机网格
|
|
|
|
Args:
|
|
rows: 行数
|
|
cols: 列数
|
|
obstacle_probability: 障碍物概率
|
|
|
|
Returns:
|
|
随机网格数据
|
|
"""
|
|
grid = []
|
|
for r in range(rows):
|
|
row = []
|
|
for c in range(cols):
|
|
if random.random() < obstacle_probability:
|
|
row.append(1) # 障碍物
|
|
else:
|
|
row.append(0) # 空地
|
|
grid.append(row)
|
|
return grid
|
|
|
|
def create_tunnel_grid(rows: int, cols: int) -> List[List[int]]:
|
|
"""
|
|
创建隧道网格（多条曲折的隧道）
|
|
|
|
Args:
|
|
rows: 行数
|
|
cols: 列数
|
|
|
|
Returns:
|
|
隧道网格数据
|
|
"""
|
|
grid = [[1 for _ in range(cols)] for _ in range(rows)] # 全部初始化为墙
|
|
|
|
# 创建主要隧道
|
|
# 水平隧道
|
|
tunnel_row = rows // 2
|
|
for c in range(1, cols - 1):
|
|
grid[tunnel_row][c] = 0
|
|
|
|
# 垂直隧道
|
|
tunnel_col = cols // 2
|
|
for r in range(1, rows - 1):
|
|
grid[r][tunnel_col] = 0
|
|
|
|
# 添加一些分支隧道
|
|
branch_positions = [(tunnel_row, tunnel_col // 4),
|
|
(tunnel_row, 3 * tunnel_col // 4),
|
|
(tunnel_row // 4, tunnel_col),
|
|
(3 * tunnel_row // 4, tunnel_col)]
|
|
|
|
for br, bc in branch_positions:
|
|
# 水平分支
|
|
for c in range(max(1, bc - 3), min(cols - 1, bc + 4)):
|
|
grid[br][c] = 0
|
|
# 垂直分支
|
|
for r in range(max(1, br - 3), min(rows - 1, br + 4)):
|
|
grid[r][bc] = 0
|
|
|
|
return grid
|
|
|
|
def get_scenario_names() -> List[str]:
|
|
"""
|
|
获取可用场景名称
|
|
|
|
Returns:
|
|
场景名称列表
|
|
"""
|
|
return ["simple", "maze", "room", "random", "tunnel"]
|
|
|
|
def create_scenario(scenario_name: str, rows: int = 20, cols: int = 20) -> List[List[int]]:
|
|
"""
|
|
创建指定场景
|
|
|
|
Args:
|
|
scenario_name: 场景名称
|
|
rows: 行数
|
|
cols: 列数
|
|
|
|
Returns:
|
|
网格数据
|
|
"""
|
|
if scenario_name == "simple":
|
|
return create_simple_grid(rows, cols)
|
|
elif scenario_name == "maze":
|
|
return create_maze_grid(rows, cols)
|
|
elif scenario_name == "room":
|
|
return create_room_grid(rows, cols)
|
|
elif scenario_name == "random":
|
|
return create_random_grid(rows, cols)
|
|
elif scenario_name == "tunnel":
|
|
return create_tunnel_grid(rows, cols)
|
|
else:
|
|
return create_simple_grid(rows, cols)
|
|
|
|
def get_suggested_start_goal(scenario_name: str, grid: List[List[int]]) -> Tuple[Tuple[int, int], Tuple[int, int]]:
|
|
"""
|
|
获取场景建议的起点和终点
|
|
|
|
Args:
|
|
scenario_name: 场景名称
|
|
grid: 网格数据
|
|
|
|
Returns:
|
|
(起点, 终点) 元组
|
|
"""
|
|
rows = len(grid)
|
|
cols = len(grid[0]) if rows > 0 else 0
|
|
|
|
if scenario_name == "simple":
|
|
return (0, 0), (rows - 1, cols - 1)
|
|
elif scenario_name == "maze":
|
|
return (1, 1), (rows - 2, cols - 2)
|
|
elif scenario_name == "room":
|
|
return (0, 0), (rows - 1, cols - 1)
|
|
elif scenario_name == "random":
|
|
# 寻找空地作为起点和终点
|
|
start = None
|
|
goal = None
|
|
for r in range(rows):
|
|
for c in range(cols):
|
|
if grid[r][c] == 0:
|
|
if start is None:
|
|
start = (r, c)
|
|
else:
|
|
goal = (r, c)
|
|
return start or (0, 0), goal or (rows - 1, cols - 1)
|
|
elif scenario_name == "tunnel":
|
|
return (1, 1), (rows - 2, cols - 2)
|
|
else:
|
|
return (0, 0), (rows - 1, cols - 1) |