def _add_peaks_from_seed_points3d(self, seed_points_3d, style='cone', steepness=1):
"""
The seed points are local or global maximums, and then
surrounding points are lower and lower with distance. 'cone' style has peaks shaped like cones,
'pyramid' style shapes them like 4 sided pyramids. Note that where peaks would collide, the valleys
are not smoothed. It is possible to have more "extraction points" (summits or flat areas) than given
in the seeds due to the way the peaks collide
:param steepness: Height of step between adjacent cells
:param seed_points_3d: e.g. [Point3D(15,6,69), Point3D(21,11,52), Point3D(3, 4, 34)]
:param style: 'cone' or 'pyramid' which determines curve shape
:return: self (to allow chaining)
"""
if style == 'cone':
z_func = Point2D.distance2d
elif style == 'pyramid':
z_func = Point2D.max_orthogonal_distance
else:
raise Exception('Unknown Style:' + style)
# Go through all points. Pick the closest seed to the point, and calculate its z value and insert it
for y in range(self._lower_left.y, self._upper_right.y + 1):
for x in range(self._lower_left.x, self._upper_right.x + 1):
grid_point = Point2D(x, y)
# consider using scipy.spatial.distance.cdist if moving to numpy. Lots of metric options to use!
closest_seed = min(seed_points_3d, key=lambda seed: grid_point.distance2d(seed))
delta_z = round(steepness * (closest_seed.z - z_func(grid_point, closest_seed)))
if delta_z < 0:
delta_z = 0
# get existing value if any for this point (in case adding a layer)
z = self._tm.get_z(grid_point,
default=0)
self._tm.set_z(Point2D(x, y), z + delta_z)
return self._tm
def test_all_points_xyz(self):
self.tm.set_z(Point2D(9, 5), 1)
self.tm.set_z(Point2D(5, 7), 5)
self.tm.set_z(Point2D(1, 6), 2)
# Note assertCountEqual below actually sees if the elements are the same, regardless of order
self.assertCountEqual([(9, 5, 1), (5, 7, 5), (1, 6, 2)],
self.tm.iter_all_points_xyz())
def test_populate_from_matrix_without_setting_bounds(self):
"""
:return:
"""
self.tm = make_example_topology(origin=Point2D(10, 20),
set_bounds=False)
self.assertNotEqual(OUT_OF_BOUNDS, self.tm.get_z(Point2D(9, 22)))
def set_z(self, point, height):
"""
Sets z value at a point to the passed height
:param point:
:param height:
"""
self._known_z[point] = height
# adjust our current bounds
if not self._upper_right:
self._upper_right = self._lower_left = point
else:
self._upper_right = Point2D(max(self._upper_right.x, point.x),
max(self._upper_right.y, point.y))
self._lower_left = Point2D(min(self._lower_left.x, point.x),
min(self._lower_left.y, point.y))
def create_adjacent_data_at_point(self, point):
val = 0
# makes a 2d array values 1-9
for y in range(point.y - 1, point.y + 2):
for x in range(point.x - 1, point.x + 2):
val += 1
self.tm.set_z(Point2D(x, y), val)
def test_populate_from_matrix(self):
self.tm = make_example_topology()
height = len(TEST_MAP)
width = len(TEST_MAP[0])
for y in range(height):
for x in range(width):
row = height - y - 1 # row is reversed from y
self.assertEqual(TEST_MAP[row][x], self.tm.get_z(Point2D(x,
y)))
def test_iter_points_to_destination(self):
point = Point2D(4, 1)
path = list(
self.navigator.iter_points_to_destination(point, self.sensors))
self.assertCountEqual(
[Point3D(4, 1, 1),
Point3D(3, 2, 2),
Point3D(2, 2, 3)], path)
def get_scan_cost_at_point(self, point):
"""
Gets the stored cost of scans at the point. Returns 0 if not scanned
:param point:
:return:
"""
pt = Point2D(point.x, point.y)
cost = self._scan_costs.get(pt, 0)
return cost
def test_scan_and_get_destination_point_candidates(self):
# we should get the point, its 8 surrounding points, and for each of those 8, their surrounding points
# the result is essentially a 5x5 grid of points centered around the origin point
point = Point2D(4, 1)
surround = to_points([(x, y) for y in range(-1, 4)
for x in range(2, 7)])
# xy = [(3, 0), (3, 1), (3, 2), (4, 0), (4, 1), (4, 2), (5, 0), (5, 1), (5, 2)]
candidates = self.navigator._scan_and_get_destination_point_candidates(
point, self.sensors)
self.assertCountEqual(surround, candidates)
def set_scan_cost_at_point(self, point, value):
"""
Stores the cost for a given point. If a point has already been scanned, then it just adds the value to the
already stored one. In reality, that should be rare, and the second scan should have value 0
:param point:
:param value:
"""
pt = Point2D(point.x, point.y)
previous = self.get_scan_cost_at_point(point)
self._scan_costs[pt] = value + previous
def test_navigate(self):
"""
System test. Tests if navigation completes, and signals are broadcast and received
:return:
"""
logging.getLogger().setLevel(level=logging.INFO)
self.drone.navigate_to_destination_point(Point2D(10, 10))
self.assertEqual(1, len(self.start))
self.assertEqual(1, len(self.destination))
extraction_point = self.destination[0].to_2d()
self.assertTrue(self.tm.is_highest_or_tie_in_radius_and_all_known(extraction_point, 1))
def __init__(self):
# random.seed(100) # for testing, we want to always genereate same map
self.tm = TopologyFactory.make_fake_topology(upper_right=Point2D(
48, 32),
density=0.0075)
laser = SimulatedTopologySensor(simulated_map=self.tm,
power_on_cost=4,
scan_point_cost=2)
# radar = SimulatedTopologySensor(simulated_map=self.tm, power_on_cost=10, scan_point_cost=0)
topology_sensors = [laser]
self.strategy_index = 0
self.move_strategy = strategies[self.strategy_index]
self.drone = DroneFactory.make_drone(move_strategy=self.move_strategy,
topology_sensors=topology_sensors)
self.last_xy = None
def _random_points_3d(self, number_of_seeds, min_z, max_z):
"""
Creates a list of unique (x,y,z) tuples
:param number_of_seeds: Number of unique points to generate
:param min_z: Minimum z value to generate
:param max_z: Maximum z value to generate
:return:
"""
# Sanity check. We can't get more seeds than what's available in the bounds
assert number_of_seeds <= self.cell_count
found = {}
while len(found) < number_of_seeds:
pt = Point2D(random.randint(self._lower_left.x, self._upper_right.x),
random.randint(self._lower_left.y, self._upper_right.y))
if pt not in found: # make sure unique
found[pt] = random.randint(min_z, max_z)
return [Point3D(pt.x, pt.y, z) for pt, z in found.items()]
def navigate(self, x, y, change_strategy=False):
if change_strategy:
if not self.last_xy:
return None
(x, y) = self.last_xy
else:
self.last_xy = (x, y)
nav = self.drone.navigator
if change_strategy:
self.strategy_change()
nav.set_move_strategy(make_move_strategy(
self.move_strategy)) # need to reset between runs
path = list(self.drone.navigate_to_destination_point(Point2D(x, y)))
points = [(pt.x, pt.y, pt.z, nav.get_scan_cost_at_point(pt))
for pt in path] # convert from Point3D list to tuple list
return points, self.move_strategy.name
def make_from_matrix(topology_matrix, origin=ORIGIN, set_bounds=True):
"""
Populates the map from a matrix. Mostly useful in testing, but someday we might want to preload one or more
matrices of data into our maps. Consider supporting numpy matrixes too
:param topology_matrix:
:param origin:
:param set_bounds: True (default) if we should limit the map's bounds to this array's dimensions
:return:
"""
width = len(topology_matrix[0]) # the width of the first row is the width of all rows
height = len(topology_matrix)
origin = origin
if set_bounds:
tm = TopologyMap(lower_left_bounds=origin, upper_right_bounds=origin.translate(width - 1, height - 1))
else:
tm = TopologyMap()
for row in range(height):
for col in range(width):
# reversing rows to make y value
point = Point2D(origin.x + col, origin.y + height - row - 1)
tm.set_z(point, topology_matrix[row][col])
return tm
def __call__(self, topology_map, point, destination):
"""
Determne next point by bisecting towards it in decreasing increments
:param topology_map:
:param point:
:param destination:
:return:
"""
next_point = super().__call__(topology_map, point, destination)
self._decrement()
if self.highest_point:
# see if if we moved downhill last time
if topology_map.get_z(
self.highest_point) > topology_map.get_z(point):
# we went downhill, so bisect back to high point
midpoint = point.midpoint_to(self.highest_point)
return Point2D(math.floor(midpoint.x), math.floor(midpoint.y))
else:
self.highest_point = point # this point is new high
else:
self.highest_point = point # initialize now that we have a point
return next_point
def make_fake_topology(density=.02, lower_left=ORIGIN, upper_right=Point2D(30, 30), max_z=None):
"""
Generates a topology to test with. It's possible that the resulting topology could have more "extraction points"
(peaks or flat areas) than the number of seeds because of the way the generated peaks collide.
This code is slow for large regions. Consider refactoring to start out with a zero'd numpy array and
manipulating that, only to make from a matrix at the end
:param density: number of peeks / total area
:param lower_left: lower-left point
:param upper_right: upper-right point
:param max_z: Maximum z value to generate
:return: generated topology map
"""
styles = ['cone', 'pyramid']
if not max_z:
biggest_axis = max(upper_right.x - lower_left.x, upper_right.y - lower_left.y)
max_z = round(biggest_axis / 1.2) # Just need a rule here. how about max height is half width?
factory = TopologyFactory(lower_left, upper_right)
number_of_seeds = round(factory.cell_count * density) # how many seeds depends on density and area
# Produce roughly (due to rounding) the number of seeds. We will perform multiple passes,
# generating random x,y,z values and adding them as peaks on the map. With each pass, the
# range of z values tends to get smaller (though there is randomness).
# Also, each pass has a random steepness value, which is essentially the step height if we
# are walking up an Aztec pyramid
seeds_per_pass = 5 # a decent-looking value
passes = round(number_of_seeds // seeds_per_pass)
for i in range(1, passes + 1):
# get a z-range for this pass
max_z_pass = round(max_z / i)
min_z_pass = max_z_pass // 2
seeds = factory._random_points_3d(seeds_per_pass, min_z_pass, max_z_pass)
steepness = random.uniform(1, 4) # the resulting step height between adjacent cells
factory._add_peaks_from_seed_points3d(seeds, random.choice(styles), steepness=steepness)
return factory._tm
def test_set_and_get_z_with_different_point_objects(self):
self.tm.set_z(Point2D(3, 3), 10)
self.assertEqual(10, self.tm.get_z(Point2D(3, 3)))
def test_get_z_unknown_point(self):
self.assertIsNone(self.tm.get_z(Point2D(3, 3)))
def to_points(list_of_points):
return [Point2D(x, y) for (x, y) in list_of_points]
def test_boundary_points(self):
self.tm.set_z(Point2D(9, 5), 1)
self.tm.set_z(Point2D(5, 7), 5)
self.tm.set_z(Point2D(1, 6), 2)
self.assertEqual((Point2D(1, 5), Point2D(9, 7)),
self.tm.boundary_points)
def test_width_and_height(self):
self.tm.set_z(Point2D(9, 5), 1)
self.tm.set_z(Point2D(5, 7), 5)
self.tm.set_z(Point2D(1, 6), 2)
# don't forget that we need to add 1 to both width and height!
self.assertEqual((9, 3), self.tm.width_and_height)
def test_determine_next_point(self):
point = Point2D(4, 1)
point = self.navigator._determine_next_point(point, self.sensors)
expecting = Point2D(3, 2)
self.assertEqual(expecting, point)
def test_on_max_bounds_y(self):
"""
:return:
"""
self.tm = make_example_topology(origin=Point2D(10, 20))
self.assertNotEqual(OUT_OF_BOUNDS, self.tm.get_z(Point2D(12, 25)))
def test_before_min_bounds_y(self):
"""
:return:
"""
self.tm = make_example_topology(origin=Point2D(10, 20))
self.assertEqual(OUT_OF_BOUNDS, self.tm.get_z(Point2D(12, 19)))
def test_after_max_bounds_x(self):
"""
:return:
"""
self.tm = make_example_topology(origin=Point2D(10, 20))
self.assertEqual(OUT_OF_BOUNDS, self.tm.get_z(Point2D(17, 22)))
def test_populate_from_matrix_origin_shift(self):
self.tm = make_example_topology(origin=Point2D(10, 20))
# The 4 is normally at point (6,4)
self.assertEqual(4, self.tm.get_z(Point2D(16, 24)))
def navigate(self, x, y):
path = self.drone.navigate_to_destination_point(Point2D(x, y))
points = [pt.to_tuple() for pt in path] # convert from Point3D list to tuple list
# print("Found destination at", path[-1])
# print(*points)
return points
"""
Illustrates the basic usage of the library
"""
# Sets the python path first in case PYTHONPATH isn't correct
import sys
sys.path.extend(['.', './src', './tests', './examples'])
from drone.drone_factory import DroneFactory
from geometry.point import Point2D
from navigation.destinations import ExtractionPoint
from navigation.move_strategy import MoveStrategyType
from sensors.simulated_topology_sensor import SimulatedTopologySensor
from topology.topology_factory import TopologyFactory
import random
SIMULATED_MAP_DIMENSIONS = Point2D(48, 32)
def navigate_to_extraction_point(start_point):
"""
Calculates the extraction point on a simulated map given a start point
:param start_point: Point2D
:return: extraction Point2D
"""
# Generate a fake topology for testing. Higher density=more bumpy
simulated_topology = TopologyFactory.make_fake_topology(
upper_right=SIMULATED_MAP_DIMENSIONS, density=0.0075)
# Make a sensor which scans a siulated topology
topology_sensors = [
SimulatedTopologySensor(simulated_map=simulated_topology,