UtamaDonny

env.py

Jun 8th, 2021 (edited)
161
262 days
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. import pygame
  2. import math
  3.  
  4. class buildEnvironment:
  5.     def __init__(self,MapDimensions):
  6.         pygame.init()
  7.         self.pointCloud=[]
  8.         self.externalMap=pygame.image.load('Robotika\SLAM\map1.png')
  9.         self.maph, self.mapw = MapDimensions
  10.         self.MapWindowName = 'Path Planning'
  11.         pygame.display.set_caption(self.MapWindowName)
  12.         self.map=pygame.display.set_mode((self.mapw,self.maph))
  13.         self.map.blit(self.externalMap,(0,0))
  14.         #colors
  15.         self.black = (0,0,0)
  16.         self.blue =(0,0,255)
  17.         self.green = (0,255,0)
  18.         self.red = (255,0,0)
  19.         self.white = (255,255,255)
  20.  
  21.     def AD2pos(self,distance,angle,robotPosition):
  22.         x=distance * math.cos(angle)+robotPosition[0]
  23.         y=-distance * math.sin(angle)+robotPosition[1]
  24.         return (int(x),int(y))
  25.  
  26.     def dataStorage(self,data):
  27.         print(len(self.pointCloud))
  28.         for element in data:
  29.             point= self.AD2pos(element[0],element[1],element[2])
  30.             if point not in self.pointCloud:
  31.                 self.pointCloud.append(point)
  32.         # return (str(False))
  33.    
  34.     def show_sensorData(self):
  35.         self.infomap=self.map.copy()
  36.         for point in self.pointCloud:
  37.             self.infomap.set_at((int(point[0]),int(point[1])),(255,0,0))
RAW Paste Data