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