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
...

多种局部规划算法

概率路线图

快速探索随机树

势场

多图搜索算法

迪杰斯特拉的最短路径

A* 搜索

贪心最佳优先搜索

图搜索可视化

随机生成障碍物并可自定义障碍物数量

拖放障碍物生成

拖放可定制的开始/结束姿势

每个规划器算法的可定制参数

概率路线图

快速探索随机树

势场

源代码

参阅 - 亚图跨际