def generate_road_map_info(self, node_x, node_y, rr, obstacle_tree):
"""
Road map generation
node_x: [m] x positions of sampled points
node_y: [m] y positions of sampled points
rr: Robot Radius[m]
obstacle_tree: KDTree object of obstacles
"""
road_map = []
n_sample = len(node_x)
node_tree = KDTree(np.vstack((node_x, node_y)).T)
for (i, ix, iy) in zip(range(n_sample), node_x, node_y):
index, dists = node_tree.search(
np.array([ix, iy]).reshape(2, 1), k=n_sample)
inds = index[0]
edge_id = []
for ii in range(1, len(inds)):
nx = node_x[inds[ii]]
ny = node_y[inds[ii]]
if not self.is_collision(ix, iy, nx, ny, rr, obstacle_tree):
edge_id.append(inds[ii])
if len(edge_id) >= self.N_KNN:
break
road_map.append(edge_id)
# plot_road_map(road_map, sample_x, sample_y)
return road_map
class BoxScenario(Scenario):
"""
Create a euclidean "box structure" scenario with a spherical robot as a test scenario
"""
MAX_EDGE_LEN = 30.0 # don't try to connect edges further than this
NUM_SAMPLES_PER_RUN = 20
def __init__(self, start, goal, llim, ulim, robot_radius, obstacles, dim=2):
"""
start:
obstacles: Only rectangular for now. np.array of (x,y) positions defining a set of obstacles
"""
self._dim = dim
start, goal = np.array(start), np.array(goal)
self._check_last_axis(start, "start")
self._check_last_axis(goal, "goal")
self._obstacle_config = obstacles
self._obstacles = self.create_obstacles(obstacles)
self._obs_kdtree = KDTree(self._obstacles)
self._robot_radius = robot_radius
self.llim = np.array(llim)
self.ulim = np.array(ulim)
self.start = start
self.goal = goal
def create_obstacles(self, obstacle_config):
# assume 2d obstacles for now
obstacle_points = []
for (start, end) in obstacle_config:
(start_x, start_y), (end_x, end_y) = start, end
for i in range(start_x, end_x + 1):
obstacle_points.append([i, start_y])
obstacle_points.append([i, end_y])
for i in range(start_y, end_y + 1):
obstacle_points.append([start_x, i])
obstacle_points.append([end_x, i])
return np.array(obstacle_points)
def _check_last_axis(self, arr, name=""):
assert arr.shape[-1] == self._dim, "Invalid input {} for box of dimension {}".format(name, self._dim)
def point_in_collision(self, point):
idxs, dist = self._obs_kdtree.search(point)
if dist <= self._robot_radius:
return True # collision
return False # OK
def is_collision(self, start, goal):
"""
is the path from start to goal in collision
"""
curr = start.copy()
diff = goal - start
d = np.linalg.norm(diff)
if d >= self.MAX_EDGE_LEN:
return True
diff /= d
nsteps = int(d // self._robot_radius)
for i in range(nsteps):
idxs, dist = self._obs_kdtree.search(curr)
if dist <= self._robot_radius:
return True # collision
curr += self._robot_radius * diff
# goal check
idxs, dist = self._obs_kdtree.search(goal)
if dist <= self._robot_radius:
return True # collision
return False # OK
@staticmethod
def get_vertices(llim, ulim):
# TODO: assume 2 dim for now
return [
[llim[0], llim[1]],
[ulim[0], llim[1]],
[llim[0], ulim[1]],
[ulim[0], ulim[1]],
]
@staticmethod
def divide_space(lower, upper, num_divisions):
"""
start: np.array (dim, ) - minimum bounds of the space to subdivide
end: np.array(dim, ) - maximum bounds of the space to subdivide
num_divisions: np.array(dim, ) - number of divisions along each axis >= 0
returns: list of (lower, upper) of new subspaces
"""
lower, upper, num_divisions = list(lower), list(upper), list(num_divisions)
subspaces = [(lower, upper)]
for i, nd in enumerate(num_divisions):
new_subspaces = []
while len(subspaces) > 0:
curr_lower, curr_upper = subspaces.pop()
increment = (curr_upper[i] - curr_lower[i]) / (nd + 1)
for j in range(nd + 1):
new_lower = curr_lower[:i] + [curr_lower[i] + increment * j] + curr_lower[i+1:]
new_upper = curr_upper[:i] + [curr_lower[i] + increment * (j+1)] + curr_upper[i+1:]
new_subspaces.append((new_lower, new_upper))
subspaces = new_subspaces
return subspaces
def sample(self, ):
samples = np.random.uniform(low=0, high=1, size=(self.NUM_SAMPLES_PER_RUN, self._dim))
samples = samples * (self.ulim - self.llim) + self.llim
collision_free = []
for sample in samples:
if self.point_in_collision(sample): continue
collision_free.append(list(sample))
return collision_free
# super profesjonalny plik z testami
from kdtree import KDTree
from sys import maxsize
# TODO dodać pomiar czasu
### TEST 1 ###
points1 = [(20, 50), (30, 40), (35, 60), (50, 100), (60, 70), (10, 45),
(10, 10), (45, 23), (7, 8), (1, 3), (18, 90), (80, 80)]
region1 = [(40, 60), (90, 110)]
answer1 = [(60, 70), (80, 80), (50, 100)]
root1 = KDTree(points1, None)
result1 = root1.search(region1[0], region1[1])
print(" ########### TEST 1 ########### ")
if sorted(result1) == sorted(answer1):
print("Correct!")
else:
print("INCORRECT")
print("Correct answer: ", end="")
print(answer1)
print("Your answer: ", end="")
print(result1)
print()
### TEST 2 ###
points2 = [(20, 50), (30, 40), (35, 60), (50, 100), (60, 70), (10, 45),
