def update(self, control_signals):
"""
The function to update solver agent position within maze. After agent position
updated it will be checked to find out if maze exit was reached afetr that.
Arguments:
control_signals: The control signals received from the control ANN
Returns:
The True if maze exit was found after update or maze exit was already
found in previous simulation cycles.
"""
if self.exit_found:
# Maze exit already found
return True
# Apply control signals
self.apply_control_signals(control_signals)
# get X and Y velocity components
vx = math.cos(geometry.deg_to_rad(
self.agent.heading)) * self.agent.speed
vy = math.sin(geometry.deg_to_rad(
self.agent.heading)) * self.agent.speed
# Update current Agent's heading (we consider the simulation time step size equal to 1s
# and the angular velocity as degrees per second)
self.agent.heading += self.agent.angular_vel
# Enforce angular velocity bounds by wrapping
if self.agent.heading > 360:
self.agent.heading -= 360
elif self.agent.heading < 0:
self.agent.heading += 360
# find the next location of the agent
new_loc = geometry.Point(x=self.agent.location.x + vx,
y=self.agent.location.y + vy)
if not self.test_wall_collision(new_loc):
self.agent.location = new_loc
# update agent's sensors
self.update_rangefinder_sensors()
self.update_radars()
# check if agent reached exit point
distance = self.agent_distance_to_exit()
self.exit_found = (distance < self.exit_range)
return self.exit_found
def update_rangefinder_sensors(self):
"""
The function to update the agent range finder sensors.
"""
for i, angle in enumerate(self.agent.range_finder_angles):
rad = geometry.deg_to_rad(angle)
# project a point from agent location outwards
projection_point = geometry.Point(
x=self.agent.location.x +
math.cos(rad) * self.agent.range_finder_range,
y=self.agent.location.y +
math.sin(rad) * self.agent.range_finder_range)
# rotate the projection point by the agent's heading angle to
# align it with heading direction
projection_point.rotate(self.agent.heading, self.agent.location)
# create the line segment from the agent location to the projected point
projection_line = geometry.Line(a=self.agent.location,
b=projection_point)
# set range to maximum detection range
min_range = self.agent.range_finder_range
# now test against maze walls to see if projection line hits any wall
# and find the closest hit
for wall in self.walls:
found, intersection = wall.intersection(projection_line)
if found:
found_range = intersection.distance(self.agent.location)
# we are interested in the closest hit
if found_range < min_range:
min_range = found_range
# Update sensor value
self.agent.range_finders[i] = min_range
def __init__(self, threshold=geometry.deg_to_rad(15)):
super().__init__()
self.threshold = threshold
self.min_direction = None
self.max_direction = None
def __init__(self, threshold=geometry.deg_to_rad(15)):
self.points = []
self.cache = {}
self.top = None
self.bottom = None
self.left = None
self.right = None
def _point_groups_are_similar(group1, group2, bounds, threshold=geometry.deg_to_rad(15)):
# measure the nearest distance between the two point groups
distance1 = geometry.manhattan(group1.start, group2.start)
distance2 = geometry.manhattan(group1.start, group2.stop)
distance3 = geometry.manhattan(group1.stop, group2.start)
distance4 = geometry.manhattan(group1.stop, group2.stop)
# reject them if they are too far apart
bad_distance = 50
too_far = all([
distance1 > bad_distance,
distance2 > bad_distance,
distance3 > bad_distance,
distance4 > bad_distance,
])
if too_far:
return False
# fit a line to each point group
xs1, ys1 = group1.xs, group1.ys
m1, b1 = geometry.Line.compute_model(xs1, ys1)
xs2, ys2 = group2.xs, group2.ys
m2, b2 = geometry.Line.compute_model(xs2, ys2)
xs = [
min(group1.start.x, group2.start.x),
max(group1.stop.x, group2.stop.x),
]
# reject them if the slopes and y-intercepts are too far apart
for x in xs:
y1 = m1 * x + b1
y2 = m2 * x + b2
dy = abs(y1 - y2)
if dy >= 5:
return False
# fit a line to the merged point group
m, b = geometry.Line.compute_model(xs1 + xs2, ys1 + ys2)
# reject if the fitted line is too different from either of the
# original point groups
if m1 - threshold > m or m1 + threshold < m:
return False
if m2 - threshold > m or m2 + threshold < m:
return False
return True
def _get_glideslope_tang(landing_point):
return METERS_PER_KILOMETER * tan(
geometry.deg_to_rad(max(MIN_GLIDESLOPE_ANGLE, landing_point[3])))
from math import tan
import utils
import geometry
METERS_PER_KILOMETER = 1000
ASC_TANG = tan(geometry.deg_to_rad(10.0)) * METERS_PER_KILOMETER
DESC_TANG = tan(geometry.deg_to_rad(10.0)) * METERS_PER_KILOMETER
TAKEOFF_ALTITUDE = 0.1 * ASC_TANG
TAKEOFF_STRAIGHT_UNTIL_ALTITUDE = 1500
MIN_GLIDESLOPE_ANGLE = 3.3
MIN_IAF_LEVEL_DISTANCE = 3.0
IAF_DISTANCE = 25.0
FAF_DISTANCE = 10.0
FLARE_DISTANCE = 0.2
GLIDESLOPE_ALTITUDE_CORRECTION = 75 * tan(
geometry.deg_to_rad(MIN_GLIDESLOPE_ANGLE))
def point_to_params(name, pt, alt=None, marker=None):
"""Returns point coordinates as a list for Kramax AutoPilot flight plan."""
if alt is None:
alt = pt[2]
params = [
('name', name),
('Vertical', 'true'),
('lat', pt[0]),
def _get_glideslope_tang(landing_point):
return METERS_PER_KILOMETER * tan(
geometry.deg_to_rad(max(MIN_GLIDESLOPE_ANGLE, landing_point[3]))
)
from math import tan
import utils
import geometry
METERS_PER_KILOMETER = 1000
ASC_TANG = tan(geometry.deg_to_rad(10.0)) * METERS_PER_KILOMETER
DESC_TANG = tan(geometry.deg_to_rad(10.0)) * METERS_PER_KILOMETER
TAKEOFF_ALTITUDE = 0.1 * ASC_TANG
TAKEOFF_STRAIGHT_UNTIL_ALTITUDE = 1500
MIN_GLIDESLOPE_ANGLE = 3.3
MIN_IAF_LEVEL_DISTANCE = 3.0
IAF_DISTANCE = 25.0
FAF_DISTANCE = 10.0
FLARE_DISTANCE = 0.2
GLIDESLOPE_ALTITUDE_CORRECTION = 75 * tan(geometry.deg_to_rad(MIN_GLIDESLOPE_ANGLE))
def point_to_params(name, pt, alt=None, marker=None):
"""Returns point coordinates as a list for Kramax AutoPilot flight plan."""
if alt is None:
alt = pt[2]
params = [
('name', name),
('Vertical', 'true'),
('lat', pt[0]),
('lon', pt[1]),
