UtamaDonny

sensors.py

Jun 8th, 2021 (edited)
140
262 days
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. import pygame
  2. import math
  3. import numpy as np
  4.  
  5. def uncertainty_add(distance, angle,sigma):
  6.     mean= np.array([distance. angle])
  7.     covariance = np.diag(sigma **2)
  8.     distance, angle=np.random.multivariate_normal(mean,covariance)
  9.     distance = max(distance,0)
  10.     angle=max(angle,0)
  11.     return [distance,angle]
  12.  
  13. class LaserSensor:
  14.     def __init__(self,Range,map,uncertainty):
  15.         self.Range=Range
  16.         self.speed= 4 #rounds/second
  17.         self.sigma = np.array([uncertainty[0], uncertainty[1]])
  18.         self.position=(0,0)
  19.         self.map=map
  20.         self.W,self.H=pygame.display.get_surface().get_size()
  21.         self.senseObstacle=[]
  22.     def distance (self,obstaclePosition):
  23.         px=(obstaclePosition[0]-self.position[0])**2
  24.         py=(obstaclePosition[1]-self.position[1])**2
  25.         return math.sqrt(px+py)
  26.  
  27.     def sense_obstacle(self):
  28.         data=[]
  29.         x1,y1 = self.position[0], self.position[1]
  30.         for angle in np.linspace(0,2*math.pi,60,False):
  31.             x2,y2=(x1+self.Range*math.cos(angle), y1-self.Range*math.sin(angle))
  32.             for i in range(0,100):
  33.                 u=i/100
  34.                 x=int(x2*u+x1*(1-u))
  35.                 y=int(y2*u+y1*(1-u))
  36.                 if 0<x<self.W and 0<y<self.H:
  37.                     color= self.map.get_at((x,y))
  38.                     if(color[0],color[1],color[2])==(0,0,0):
  39.                         distance=self.distance((x,y))
  40.                         output=uncertainty_add(distance,angle,self.sigma)
  41.                         output.append(self.position)
  42.                         data.append(output)
  43.                         break
  44.                 if len(data)>0:
  45.                     return data
  46.                     #return int(data)
  47.                 else:
  48.                     return False
  49.                     #return (str(False))
RAW Paste Data