Python和Pygame绘制自动驾驶和移动机器本地规划器算法

可视化自动驾驶车辆路径规划和移动机器人中使用的众多不同的本地规划器算法。该应用程序提供可定制的参数，以更好地了解每种算法的内部工作原理并探索它们的优点和缺点。它是用 Python 编写的，并使用 Pygame 来渲染可视化。

基类

import sys
import threadingimport pygame, random, pygame_gui
from pygame.locals import *
from planners.planners import ProbabilisticRoadmap, Color, RRT, PotentialField, CircularObstacle
from search.search import Dijkstra, AStar, GreedyBFSclass State:PRM = 0RRT = 1PF = 2def sample_envir(map_pos, map_dim, obs_dim):sx = map_pos[0]sy = map_pos[1]ex = sx + map_dim[0] - obs_dim[0]ey = sy + map_dim[1] - obs_dim[1]x = int(random.uniform(sx, ex))y = int(random.uniform(sy, ey))return (x, y)def localize(map, pos):return pos[0] - map[0], pos[1] - map[1]def generate_obs(num_obstacles, map_pos, map_dim, obs_dim):obs = []for i in range(num_obstacles):rect = Nonecollision = Truewhile collision:pos = sample_envir(map_pos, map_dim, obs_dim)size = (int(random.uniform(10, obs_dim[0])), int(random.uniform(10, obs_dim[1])))rect = pygame.Rect(pos, size)collision = Falsefor obj in obs:if rect.colliderect(obj):collision = Truebreakobs.append(rect)return obsdef generate_circle_obs(num_obstacles, map_pos, map_size, circle_obs_dim, goal_pose):obs = []for i in range(num_obstacles):collision = Truewhile collision:pos = sample_envir(map_pos, map_size, (circle_obs_dim,circle_obs_dim))rad = int(random.uniform(10, circle_obs_dim))circle = CircularObstacle(*pos,rad)collision = circle.collidepoint(goal_pose)obs.append(circle)return obs
...