Python和Pygame绘制自动驾驶和移动机器本地规划器算法-Toy模板网

这篇具有很好参考价值的文章主要介绍了Python和Pygame绘制自动驾驶和移动机器本地规划器算法。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

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

基类

import sys
import threading

import pygame, random, pygame_gui
from pygame.locals import *
from planners.planners import ProbabilisticRoadmap, Color, RRT, PotentialField, CircularObstacle
from search.search import Dijkstra, AStar, GreedyBFS


class State:
    PRM = 0
    RRT = 1
    PF = 2

def 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 = None
        collision = True
        while 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 = False
            for obj in obs:
                if rect.colliderect(obj):
                    collision = True
                    break
        obs.append(rect)
    return obs


def generate_circle_obs(num_obstacles, map_pos, map_size, circle_obs_dim, goal_pose):
    obs = []
    for i in range(num_obstacles):
        collision = True
        while 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
...