🍠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* 搜索

贪心最佳优先搜索

图搜索可视化

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

拖放障碍物生成

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

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

概率路线图

快速探索随机树

势场

源代码

PreviousDijkstra算法 | 迪杰斯特拉算法-迷宫解算器可视化 NextPython简单线性回归算法实现及应用示例

Last updated 11 months ago

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