learn_slam/slam/environment.py

from collections import namedtuple

import pygame

RGB_COLOR_CODES = [
    (0, 0, 0),  # black
    (70, 70, 70),  # grey
    (0, 0, 255),  # blue
    (0, 255, 0),  # green
    (255, 0, 0),  # red
    (255, 255, 255)  # white
]

Colors = namedtuple("Colors", "black,grey,blue,green,red,white", defaults=RGB_COLOR_CODES)


class Environment:

    def __init__(self, map_path: str, map_height: int, map_width: int, window_name: str = "RRT path planning",
                 colors: Colors = None):
        self.point_cloud = []
        self.external_map = pygame.image.load(map_path)
        self.map_height = map_height
        self.map_width = map_width
        self.window_name = window_name
        self.colors = colors if colors else Colors()
        self._initialize_pygame()

    def _initialize_pygame(self):
        pygame.init()
        pygame.display.set_caption(self.window_name)
        self.map = pygame.display.set_mode((self.map_width, self.map_height))
        # overlay external map
        self.map.blit(self.external_map, (0, 0))