You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
89 lines
3.8 KiB
Python
89 lines
3.8 KiB
Python
"""Module containing simulated robot sensors"""
|
|
import math
|
|
|
|
import numpy as np
|
|
|
|
|
|
def add_noise(distance, angle, sigma):
|
|
_mean = np.array([distance, angle])
|
|
# Noise for distance or angle are not correlated
|
|
covariance = np.diag(sigma ** 2)
|
|
new_distance, new_angle = np.random.multivariate_normal(_mean, covariance)
|
|
# Don't want negative values
|
|
new_distance = max(new_distance, 0)
|
|
new_angle = max(new_angle, 0)
|
|
return [new_distance, new_angle]
|
|
|
|
|
|
class Lidar2DSensor:
|
|
|
|
def __init__(self, senor_range, environment_map, uncertainty, speed=4, colors=None):
|
|
"""
|
|
:param range: range of the sensor
|
|
:param map: the map
|
|
:param uncertainty: uncertainty of sensor measurements
|
|
:param speed: revolutions per second
|
|
"""
|
|
self.range = senor_range
|
|
self.map = environment_map
|
|
self._uncertainty = uncertainty
|
|
self.speed = speed
|
|
# The sensor noise represented as a distance and an angle
|
|
self.sigma = np.array([self._uncertainty[0], self._uncertainty[1]])
|
|
# TODO make this a value of the robot.
|
|
self.position = (0, 0)
|
|
self.map_width, self.map_height = self.map.get_size()
|
|
self.sensor_point_cloud = []
|
|
self.colors = colors
|
|
|
|
def calc_distance(self, obstacle_position):
|
|
"""Calculates the distance from robot position to obstacle.
|
|
|
|
Uses the Euclidean distance calculation.
|
|
distance = sqrt((Xb - Xa)^2 + (Yb-Ya)^2)
|
|
"""
|
|
px = (obstacle_position[0] - self.position[0]) ** 2
|
|
py = (obstacle_position[1] - self.position[1]) ** 2
|
|
return math.sqrt(px + py)
|
|
|
|
def sense_obstacles(self):
|
|
data = []
|
|
x1, y1 = self.position[0], self.position[1]
|
|
|
|
# 360 degrees == 2Pi radians
|
|
# np.linspace's use allows up to set the number of values which affects the simulated map resolution.
|
|
for angle in np.linspace(0, 2 * math.pi, 60, False):
|
|
# Calculate the end point of the lidar range's line segment in the 2D plane
|
|
x2, y2 = (x1 + self.range * math.cos(angle), y1 - self.range * math.sin(angle))
|
|
# Sample the line segment and detext if the sampled point is "black" when overlayed on the simulated map
|
|
for i in range(0, 100):
|
|
# TODO break this out to a function
|
|
u = i / 100
|
|
# TODO add comment explaining this calculation
|
|
x = int(x2 * u + x1 * (1 - u))
|
|
y = int(y2 * u + y1 * (1 - u))
|
|
# TODO an optimization could be performed to not sample any further
|
|
# if the point has fallen off the map.
|
|
# Check if the sampled point is on the map
|
|
if 0 < x < self.map_width and 0 < y < self.map_height:
|
|
map_point_color = self.map.get_at((x, y))
|
|
if (map_point_color[0], map_point_color[1], map_point_color[2]) == (0, 0, 0):
|
|
# if black add some noise
|
|
distance = self.calc_distance((x, y))
|
|
noised_distance = add_noise(distance, angle, self.sigma)
|
|
# TODO there is definitely a better datastructure to use. Maybe a named tuple?
|
|
noised_distance.append(self.position)
|
|
data.append(noised_distance)
|
|
# if obstacle found stop further sampling this line
|
|
break
|
|
else:
|
|
# line fell off of map break this loop iteration
|
|
break
|
|
# TODO OMG I hate this guys code
|
|
# Callers of this function need to check truthiness not in the return statement
|
|
# if len(data)>0:
|
|
# return data
|
|
# else:
|
|
# return False
|
|
return data
|