def create_cone(point, radio, angle, opening, resolution=1):
# Define the list for the points of the cone-shape polygon
p = []
# The fisrt point will be the vertex of the cone
p.append(vis.Point(point[0], point[1]))
# Define the start and end of the arc
start = angle - opening
end = angle + opening
for i in range(start, end, resolution):
# Convert start angle from degrees to radians
rad = math.radians(i)
# Calculate the off-set of the first point of the arc
x = radio * math.cos(rad)
y = radio * math.sin(rad)
# Add the off-set to the vertex point
new_x = point[0] + x
new_y = point[1] + y
# Add the first point of the arc to the list
p.append(vis.Point(new_x, new_y))
# Add the last point of the arc
rad = math.radians(end)
x = radio * math.cos(rad)
y = radio * math.sin(rad)
new_x = point[0] + x
new_y = point[1] + y
p.append(vis.Point(new_x, new_y))
return vis.Polygon(p)
def find_shortest_path(self):
base_energy_start = self.aggregated_power_lower[0][1]
base_energy_end = self.aggregated_power_lower[-2][1]
if self.strategy == 'full-empty':
strategy_state = (1, 0)
elif self.strategy == 'full-full':
strategy_state = (1, 1)
elif self.strategy == 'empty-empty':
strategy_state = (0, 0)
elif isinstance(self.strategy, tuple):
strategy_state = self.strategy
else:
print('This operation strategy is not supported!')
self.start_energy = base_energy_start + strategy_state[
0] * self.battery_capacity * (1 - self.DOD)
# TODO this is hard coded
self.end_energy = base_energy_end + strategy_state[
1] * self.battery_capacity
start = vis.Point(0, self.start_energy)
end = vis.Point(self.time - 1, self.end_energy)
start.snap_to_boundary_of(self.env, self.epsilon)
start.snap_to_vertices_of(self.env, self.epsilon)
vis_poly = vis.Visibility_Polygon(start, self.env, self.epsilon)
shortest_path = self.env.shortest_path(start, end, self.epsilon)
return shortest_path
def get_instance():
sites = [(600, 550), (300, 400), (50, 100), (800, 100), (950, 980)]
objects, elements, corners = [], [], []
wall = [(0, 0), (1000, 0), (1000, 1000), (0, 1000)]
objects.append(wall)
elements.append(vis.Polygon([vis.Point(x, y) for x, y in wall]))
hole1 = [(100, 300), (100, 500), (150, 500), (150, 300)]
objects.append(hole1)
elements.append(vis.Polygon([vis.Point(x, y) for x, y in hole1]))
corners += hole1
hole2 = [(300, 700), (300, 800), (550, 800), (550, 700)]
objects.append(hole2)
elements.append(vis.Polygon([vis.Point(x, y) for x, y in hole2]))
corners += hole2
hole3 = [(700, 300), (700, 650), (850, 650), (850, 300)]
objects.append(hole3)
elements.append(vis.Polygon([vis.Point(x, y) for x, y in hole3]))
corners += hole3
hole4 = [(300, 100), (300, 200), (450, 200), (450, 100)]
objects.append(hole4)
elements.append(vis.Polygon([vis.Point(x, y) for x, y in hole4]))
corners += hole4
env = vis.Environment(elements)
return sites, objects, env, corners
def visibleVertices(curr_poly_vx, vertex_list_per_poly, orig_poly, j):
vs = [v for i, v in orig_poly.vertex_list_per_poly[0]]
pts = list(map(lambda x: vis.Point(x[0], x[1]), vs))
wall_x = [pt.x() for pt in pts]
wall_y = [pt.y() for pt in pts]
wall_x.append(pts[0].x())
wall_y.append(pts[0].y())
def get_vis_form_from_vx_list(vs):
poly = list(map(lambda x: vis.Point(x[1][0], x[1][1]), vs))
walls = vis.Polygon(poly)
walls.enforce_standard_form()
walls.eliminate_redundant_vertices()
return walls
env_walls = [
get_vis_form_from_vx_list(vxs)
for vxs in orig_poly.vertex_list_per_poly
]
# point from which to calculate visibility
p1 = curr_poly_vx[j][1]
vp1 = vis.Point(p1[0], p1[1])
# Create environment, wall will be the outer boundary because
# is the first polygon in the list. The other polygons will be holes
env = vis.Environment(env_walls)
# Necesary to generate the visibility polygon
vp1.snap_to_boundary_of(env, EPSILON)
vp1.snap_to_vertices_of(env, EPSILON)
isovist = vis.Visibility_Polygon(vp1, env, EPSILON)
vvs = [(isovist[i].x(), isovist[i].y()) for i in range(isovist.n())]
if DEBUG:
print(curr_poly_vx)
print("visibility polygon is:", vvs)
point_x, point_y = save_print(isovist)
point_x.append(isovist[0].x())
point_y.append(isovist[0].y())
plt.plot(wall_x, wall_y, 'black')
plt.plot(point_x, point_y, 'r')
plt.plot([vp1.x()], [vp1.y()], 'go')
plt.savefig("viz_test.png")
plt.clf()
visibleVertexSet = []
for poly_vx in vertex_list_per_poly:
curr_vis_vx = []
for i in range(len(poly_vx)):
p = vis.Point(*poly_vx[i][1])
vp = copy(p).projection_onto_boundary_of(isovist)
if (vis.distance(vp, p) <
EPSILON) and poly_vx[i][0] != curr_poly_vx[j][0]:
curr_vis_vx.append(i)
if DEBUG:
print('vertices', curr_vis_vx, 'visible from', j)
visibleVertexSet.append(curr_vis_vx)
return visibleVertexSet
def environments(epsilon=0.0000001):
wall_choords = [(0, 0), (700, 0), (700, 900), (0, 900)]
wall = vis.Polygon([vis.Point(x, y) for x, y in wall_choords])
holes_choords = [[(100, 300), (100, 500), (150, 500), (150, 300)],
[(300, 300), (300, 500), (400, 550), (400, 300)],
[(90, 700), (250, 750), (220, 600), (150, 600)],
[(330, 700), (330, 800), (530, 850), (530, 790)],
[(230, 50), (250, 90), (390, 90), (390, 50)]]
holes = []
for hc in holes_choords:
holes.append(vis.Polygon([vis.Point(x, y) for x, y in hc]))
draw_env(wall_choords, holes_choords)
env = vis.Environment([wall] + holes)
return env, vis.Visibility_Graph(env, epsilon)
def visible_area(self, x: float, y: float):
"""
Computes the visible area from point (x,y)
:param x: x coordinate of view point
:param y: y coordinate of view point
:return: Visibile area from (x,y)
"""
# Define the point of the "observer"
observer = vis.Point(x, y)
# Necessary to generate the visibility polygon
observer.snap_to_boundary_of(self.env, self.epsilon)
observer.snap_to_vertices_of(self.env, self.epsilon)
# Obtain the visibility polygon of the 'observer' in the environment
# previously defined
isovist = vis.Visibility_Polygon(observer, self.env, self.epsilon)
points = []
for i in range(isovist.n()):
points.append((isovist[i].x(), isovist[i].y()))
poly = Polygon(points)
return poly
def __get_vis(self):
poly = []
for p in self.path.to_polygons():
poly.append(vis.Polygon([vis.Point(*v) for v in p[:-1][::-1]]))
env = vis.Environment(poly)
vg = vis.Visibility_Graph(env, self.epsilon)
return env, vg
def filter_points(self,points):
res_points = []
for px,py in points:
vpoint = vis.Point(px,py)
if vpoint._in(self.environment,EPSILON):
res_points.append((px,py))
return res_points
def find_obstacles(occ_grid, thresh=125, unknown=0):
# occ_grid needs to be cv2 object
# returns list of obstacles (list of polygons)
# threshold is a little low because I was trying to exclude unknown areas as obstacles
grid = occ_grid.copy()
grid[np.where(grid == unknown)] = 255
thresh_grid = cv2.threshold(grid, thresh, 255, cv2.THRESH_BINARY)[1]
thresh_grid = np.array(thresh_grid, dtype=np.uint8)
# cnts = cv2.findContours(thresh.copy(), cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
cnts = cv2.findContours(thresh_grid, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
cnts = imutils.grab_contours(cnts)
# set contours as visilibity polygons
obstacles = []
for c in cnts:
points = []
for p in c:
points.append(vis.Point(float(p[0][0]), float(p[0][1])))
obstacle = vis.Polygon(points)
obstacle.eliminate_redundant_vertices(redundant_eps)
obstacles.append(obstacle)
return obstacles
def __init__(self,blocker_polygons,width,height):
self.environment = vis.Environment()
for poly in blocker_polygons:
points = [vis.Point(x,y) for x,y in poly]
#self.polys.append(vis.Polygon(points))
poly = vis.Polygon(points)
if poly.area() > 0:
poly.reverse()
self.environment.add_hole(poly)
self.environment.set_outer_boundary(vis.Polygon([
vis.Point(0,0),
vis.Point(width,0),
vis.Point(width,height),
vis.Point(0,height),
]))
def outer(y_up, y_down, x_left, x_right): ## use arena coordinates
#list points ccw
a = vis.Point(x_left, y_size - (y_down - 1))
b = vis.Point(x_right, y_size - (y_down - 1))
c = vis.Point(x_right, y_size - (y_up - 1))
d = vis.Point(x_left, y_size - (y_up - 1))
# Values for graph
wall_x = [a.x(), b.x(), c.x(), d.x(), a.x()]
wall_y = [a.y(), b.y(), c.y(), d.y(), a.y()]
# Create the outer boundary polygon
walls = vis.Polygon([a, b, c, d])
return wall_x, wall_y, walls
def paint_cpoint_on_img(img,
vis_env,
obstacles,
inspection_points,
c_point,
scale=50,
eps=1e-7,
max_wall=200,
fov=np.pi / 2):
# compute end-point for vertex
links_val = np.rint(compute_links(c_point) * scale).astype(int)
ee_val = links_val[-1]
# get orientation of end effector
ee_orientation = c_point.sum()
# set visibility triangle
x1 = ee_val[0] + max_wall * np.cos(ee_orientation + 0.5 * fov)
y1 = ee_val[1] + max_wall * np.sin(ee_orientation + 0.5 * fov)
x2 = ee_val[0] + max_wall * np.cos(ee_orientation - 0.5 * fov)
y2 = ee_val[1] + max_wall * np.sin(ee_orientation - 0.5 * fov)
vis_tri = Polygon([tuple(ee_val), (x1, y1), (x2, y2)])
# define observer
if is_in_bounds(ee_val):
observer = vis.Point(float(ee_val[0]), float(ee_val[1]))
observer.snap_to_boundary_of(vis_env, eps)
observer.snap_to_vertices_of(vis_env, eps)
isovist = vis.Visibility_Polygon(observer, vis_env, eps)
# get environment in points
point_x, point_y = save_print(isovist)
if len(point_x) == 0 or len(point_y) == 0:
print('No points for visibility polygon!')
return None
point_x.append(isovist[0].x())
point_y.append(isovist[0].y())
poly = Polygon([(x, y) for (x, y) in zip(point_x, point_y)])
visilbe_poly = poly.intersection(vis_tri)
if type(visilbe_poly) == Polygon and len(
list(visilbe_poly.exterior.coords)) > 0:
visilbe_poly_pts = np.array(list(
visilbe_poly.exterior.coords)).reshape((-1, 1, 2)).astype(int)
# draw visilbe polygon of the observer
cv2.fillPoly(img, [visilbe_poly_pts], 150)
# draw obstacles and inspection points
obstacles.apply(lambda x: cv2.rectangle(img, (x['x'], x['y']), (x[
'x'] + x['width'], x['y'] + x['length']), 255, -1),
axis=1)
inspection_points.apply(
lambda x: cv2.rectangle(img, (x['x'], x['y']),
(x['x'], x['y']), 100, -1),
axis=1)
return img
def hole(y_up, y_down, x_left, x_right): ## use arena coordinates
#points are listed in cc order
#list points cw
a = vis.Point(x_left, y_size - (y_down - 1))
b = vis.Point(x_left, y_size - (y_up - 1))
c = vis.Point(x_right, y_size - (y_up - 1))
d = vis.Point(x_right, y_size - (y_down - 1))
# values for graph\
hole_x = [a.x(), b.x(), c.x(), d.x(), a.x()]
hole_y = [a.y(), b.y(), c.y(), d.y(), a.y()]
# Create the hole polygon
hole = vis.Polygon([a, b, c, d])
return hole_x, hole_y, hole
def __add_sites(self, s, nx_vg, pos, env):
node = nx_vg.number_of_nodes()
pos[node] = tuple(s)
isovist = vis.Visibility_Polygon(vis.Point(*s), env, self.epsilon)
for i in xrange(isovist.n()):
p = isovist[i]
p.snap_to_vertices_of(env, self.epsilon)
_p = (p.x(), p.y())
for k, v in pos.items():
if _p == v:
nx_vg.add_edge(node, k, weight=self.__dist(s, _p))
return node
def construct_environment_demo(power_dataframe, battery_capacity):
aggregated_power = power_dataframe.cumsum().Power
aggregated_power_lower = []
aggregated_power_higher = []
i = 0
for power in aggregated_power.tolist():
aggregated_power_lower.append([i, power])
aggregated_power_higher.append([i, power + battery_capacity])
i += 1
# Construct the upper hole in a reverse order
aggregated_power_higher.reverse()
aggregated_power_higher.insert(0, aggregated_power_higher[-1])
aggregated_power_higher.insert(
1, [aggregated_power_higher[0][0], aggregated_power_higher[1][1]])
del aggregated_power_higher[-1]
# Construct the lower hole in the reverse order
aggregated_power_lower.append(
[aggregated_power_lower[-1][0], aggregated_power_lower[0][1]])
# close the hole
# aggregated_power_lower.append(aggregated_power_lower[0])
# Construct the wall list
wall_list = []
wall_list.append([aggregated_power_lower[0][0], 0])
wall_list.append([aggregated_power_lower[-1][0], 0])
wall_list.append(aggregated_power_higher[2])
wall_list.append(aggregated_power_higher[1])
higher_hole_points = [vis.Point(x, y) for x, y in aggregated_power_higher]
lower_hole_points = [vis.Point(x, y) for x, y in aggregated_power_lower]
wall_points = [vis.Point(x, y) for x, y in wall_list]
higher_hole = vis.Polygon(higher_hole_points)
lower_hole = vis.Polygon(lower_hole_points)
wall = vis.Polygon(wall_points)
return wall, higher_hole, lower_hole
def build_fixed_env(self):
# Define the points which will be the outer boundary of the environment
# Must be COUNTER-CLOCK-WISE(ccw)
# p1 = vis.Point(0-self.epsilon*2.0, 0-self.epsilon*2.0)
# p2 = vis.Point(self.workspace[0]+self.epsilon*2.0, 0-self.epsilon*2.0)
# p3 = vis.Point(self.workspace[0]+self.epsilon*2.0, self.workspace[1]+self.epsilon*2.0)
# p4 = vis.Point(0-self.epsilon*2.0,self.workspace[1]+self.epsilon*2.0)
p1 = vis.Point(0 - 5, 0 - 5)
p2 = vis.Point(self.workspace[0] + 5, 0 - 5)
p3 = vis.Point(self.workspace[0] + 5, self.workspace[1] + 5)
p4 = vis.Point(0 - 5, self.workspace[1] + 5)
# Load the values of the outer boundary polygon in order to draw it later
self.wall_x = [p1.x(), p2.x(), p3.x(), p4.x(), p1.x()]
self.wall_y = [p1.y(), p2.y(), p3.y(), p4.y(), p1.y()]
# Outer boundary polygon must be COUNTER-CLOCK-WISE(ccw)
# Create the outer boundary polygon
walls = vis.Polygon([p1, p2, p3, p4])
self.holes_array.append(walls)
for (obs, boundary) in iter(self.obs.items()):
xx, yy = boundary.exterior.coords.xy
hole_x = []
hole_y = []
point_array = []
for i in range(len(xx)):
point = vis.Point(xx[i], yy[i])
hole_x.append(point.x())
hole_y.append(point.y())
if i != (len(xx) - 1):
point_array.append(point)
hole = vis.Polygon(point_array)
# print('Hole in standard form: ',hole.is_in_standard_form())
self.holes_array.append(hole)
self.hole_x_array.append(hole_x)
self.hole_y_array.append(hole_y)
def vis_graph(poly):
P = poly_list(poly)
vis_polygons = []
for p in P:
vis_polygons.append(vis.Polygon([vis.Point(x, y) for x, y in p]))
env = vis.Environment(vis_polygons)
vg = vis.Visibility_Graph(env, 0.000001)
edges = []
for i, j in itertools.combinations(range(vg.n()), 2):
if vg(i, j):
edges.append([[env(i).x(), env(i).y()], [env(j).x(), env(j).y()]])
plt.gca().add_collection(mc.LineCollection(edges, color='g'))
print len(edges)
def __collinear(self, o, p, q):
s1 = vis.Line_Segment(vis.Point(*o), vis.Point(*p))
if vis.Point(*q).in_relative_interior_of(s1, self.eps):
return True
s2 = vis.Line_Segment(vis.Point(*o), vis.Point(*q))
if vis.Point(*p).in_relative_interior_of(s2, self.eps):
return True
return False
def visibility_polygon(obsv_x, obsv_y, obstacles):
# obsv_pt - (px,py), pixel in costmap
observer = vis.Point(obsv_x, obsv_y)
env = vis.Environment(obstacles)
observer.snap_to_boundary_of(env, epsilon)
observer.snap_to_vertices_of(env, epsilon)
# get visibility polygon
isovist = vis.Visibility_Polygon(observer, env, epsilon)
if isovist.n() == 0:
logger.warn('Visibility polygon is empty for ({:.2f},{:.2f})!'.format(
obsv_x, obsv_y))
isovist.eliminate_redundant_vertices(redundant_eps)
return isovist # change to isocnt? externally convert to shapely
def vis_graph(poly):
P = poly_list(poly)
vis_polygons = []
for p in P:
vis_polygons.append(vis.Polygon([vis.Point(x, y) for x, y in p]))
env = vis.Environment(vis_polygons)
vg = vis.Visibility_Graph(env, 0.000001)
edges = []
G = nx.Graph()
for i, j in itertools.combinations(range(vg.n()), 2):
if vg(i, j):
edges.append([[env(i).x(), env(i).y()], [env(j).x(), env(j).y()]])
G.add_edge(i,
j,
weight=np.linalg.norm(
np.array(edges[-1][0]) - np.array(edges[-1][1])))
plt.gca().add_collection(mc.LineCollection(edges, color='g'))
s, t = get_st(env)
sp = [i for i in nx.shortest_path(G, s, t, 'weight')]
sp_edges = [[[env(i).x(), env(i).y()], [env(j).x(), env(j).y()]]
for i, j in zip(sp[:-1], sp[1:])]
plt.gca().add_collection(mc.LineCollection(sp_edges, color='r', lw=2))
def get_geodesic_target(self,
rob_pos,
target,
node_landmark,
avoid_target=[]):
holes_array = self.holes_array.copy()
hole_x_array = self.hole_x_array.copy()
hole_y_array = self.hole_y_array.copy()
# start = datetime.datetime.now()
# Define the point of the "observer"
observer = vis.Point(rob_pos[0], rob_pos[1])
# Now we define some holes for our environment. A hole blocks the
# observer vision, it works as an obstacle in his vision sensor.
# The newly defined holes in this part are tha landmarks that need
# to be avoided.
for avoid, dist in avoid_target:
avoid_x = node_landmark.landmark[avoid][0][0]
avoid_y = node_landmark.landmark[avoid][0][1]
dist_buffer = dist + self.epsilon
if math.sqrt((rob_pos[0] - avoid_x)**2 +
(rob_pos[1] - avoid_y)**2) < dist_buffer + 10:
if math.sqrt((target[0] - avoid_x)**2 +
(target[1] - avoid_y)**2) < dist_buffer:
return rob_pos
hole_x = []
hole_y = []
point_array = []
theta = -math.pi
for i in range(self.discretization + 1):
xx = dist_buffer * math.cos(theta) + avoid_x
yy = dist_buffer * math.sin(theta) + avoid_y
theta = theta - 2 * math.pi / self.discretization
pt = vis.Point(round(xx, 2), round(yy, 2))
hole_x.append(pt.x())
hole_y.append(pt.y())
if i != (self.discretization):
point_array.append(pt)
hole = vis.Polygon(point_array)
# print('Hole in standard form: ',hole.is_in_standard_form())
holes_array.append(hole)
hole_x_array.append(hole_x)
hole_y_array.append(hole_y)
# Create environment, wall will be the outer boundary because
# is the first polygon in the list. The other polygons will be holes
env = vis.Environment(holes_array)
# Check if the environment is valid
# print('Environment is valid : ',env.is_valid(epsilon))
# Define another point, could be used to check if the observer see it, to
# check the shortest path from one point to the other, etc.
end = vis.Point(target[0], target[1])
# Necesary to generate the visibility polygon
observer.snap_to_boundary_of(env, self.epsilon)
observer.snap_to_vertices_of(env, self.epsilon)
# Obtein the visibility polygon of the 'observer' in the environmente
# previously define
isovist = vis.Visibility_Polygon(observer, env, self.epsilon)
# Uncomment the following line to obtein the visibility polygon
# of 'end' in the environmente previously define
#polygon_vis = vis.Visibility_Polygon(end, env, epsilon)
# Obtein the shortest path from 'observer' to 'end' and 'end_visible'
# in the environment previously define
shortest_path = env.shortest_path(observer, end, self.epsilon)
route = shortest_path.path()
path_x = []
path_y = []
# print ('Points of Polygon: ')
for i in range(len(route)):
x = route[i].x()
y = route[i].y()
path_x.append(x)
path_y.append(y)
# Print the length of the path
"""
print("\nx\nx\nx\n")
print("Shortest Path length from observer to end: ", shortest_path.length())
print("\nx\nx\nx\n")
# Check if 'observer' can see 'end', i.e., check if 'end' point is in
# the visibility polygon of 'observer'
print( "Can observer see end? ", end._in(isovist, self.epsilon))
"""
# Print the point of the visibility polygon of 'observer' and save them
# in two arrays in order to draw the polygon later
point_x, point_y = self.save_print(isovist)
# Add the first point again because the function to draw, draw a line from
# one point to the next one and to close the figure we need the last line
# from the last point to the first one
point_x.append(isovist[0].x())
point_y.append(isovist[0].y())
if self.plot_images:
# Set the title
p.title('VisiLibity Test')
# Set the labels for the axis
p.xlabel('X Position')
p.ylabel('Y Position')
# Plot the outer boundary with black color
p.plot(self.wall_x, self.wall_y, 'black')
# Plot the position of the observer with a green dot ('go')
p.plot([observer.x()], [observer.y()], 'go')
# Plot the position of 'end' with a green dot ('go')
p.plot([end.x()], [end.y()], 'go')
# Plot the position of 'end_visible' with a green dot ('go')
# Plot the visibility polygon of 'observer'
p.plot(point_x, point_y)
p.plot(path_x, path_y, 'b')
for i in range(len(hole_x_array)):
p.plot(hole_x_array[i], hole_y_array[i], 'r')
# Show the plot
p.show()
"""
print("\nx\nx\nx\n")
print("Shortest Path length from observer to end: ", shortest_path.length())
print("\nx\nx\nx\n")
print ('Points of Polygon: ')
for i in range(len(route)):
x = path_x[i]
y = path_y[i]
print("%f, %f" %(x,y))
print("\nx\nx\nx\n")
print("\nx\nx\nx\n")
print("\nx\nx\nx\n")
print("\nx\nx\nx\n")
"""
# NBA_time = (datetime.datetime.now() - start).total_seconds()
# print('Time for constructing the NBA: {0:.4f} s'.format(NBA_time))
#If start and goal are within threshold distance then return the start point itself
if len(path_x) < 2:
return path_x[0], path_y[0]
return path_x[1], path_y[1]
def save_images_for_path(data_dir, trial_idx, p_zb, scale=50, eps=1e-7):
max_wall = 200
fov = np.pi / 2
files_list = {'obstacles': os.path.join(data_dir, f'test_p_zb_{trial_idx}_{p_zb}_obstacles.csv'),
'inspection_points': os.path.join(data_dir, f'test_p_zb_{trial_idx}_{p_zb}_inspection_points.csv'),
'configurations': os.path.join(data_dir, f'test_p_zb_{trial_idx}_{p_zb}_conf'),
'vertex': os.path.join(data_dir, f'test_p_zb_{trial_idx}_{p_zb}_vertex'),
'results':os.path.join(data_dir, f'test_search_p_zb_{trial_idx}_{p_zb}_result')}
# check if test files exist
broken_files = False
for file_path in files_list.values():
if not os.path.isfile(file_path):
broken_files = True
if broken_files:
print('Broken file')
sys.exit()
# construct image with obstacles
img = np.zeros((101,101))
obstacles = (pd.read_csv(files_list['obstacles']) * scale).round(0).astype(int)
obstacles_columns = obstacles.columns.to_list()
obstacles['p1'] = obstacles[obstacles_columns].astype(float).apply(lambda x: vis.Point(x['x'],x['y']),axis=1)
obstacles['p2'] = obstacles[obstacles_columns].astype(float).apply(lambda x: vis.Point(x['x']+x['width'],x['y']),axis=1)
obstacles['p3'] = obstacles[obstacles_columns].astype(float).apply(lambda x: vis.Point(x['x']+x['width'],x['y']+x['length']),axis=1)
obstacles['p4'] = obstacles[obstacles_columns].astype(float).apply(lambda x: vis.Point(x['x'],x['y']+x['length']),axis=1)
obstacles['polygon'] = obstacles.apply(lambda x: vis.Polygon([x['p1'], x['p2'], x['p3'], x['p4']]) ,axis=1)
# define bounded environment with obstacles
p1 = vis.Point(0,0)
p2 = vis.Point(101,0)
p3 = vis.Point(101,101)
p4 = vis.Point(0,101)
walls_poly = vis.Polygon([p1, p2, p3, p4])
vis_env = vis.Environment([walls_poly] + obstacles['polygon'].to_list())
# load inspection points csv
inspection_points = (pd.read_csv(files_list['inspection_points']) * scale).round(0).astype(int)
# load configuration space csv
cspace_df = pd.read_csv(files_list['configurations'], delimiter=' ', header=None).drop(columns=[0,6])
# drop already seen points from inspection points
with open(files_list['results'],'r') as f:
line = f.readlines()[-2]
vertices_idxs = [int(x) for x in line.split(' ')[1:-1]]
img_copy = img.copy()
for j in range(0,len(vertices_idxs)-1):
c_point = cspace_df.iloc[j].to_numpy()
# compute end-point for vertex
links_val = np.rint(compute_links(c_point) * scale).astype(int)
ee_val = links_val[-1]
# get orientation of end effector
ee_orientation = c_point.sum()
# set visibility triangle
x1 = ee_val[0] + max_wall * np.cos(ee_orientation + 0.5 * fov)
y1 = ee_val[1] + max_wall * np.sin(ee_orientation + 0.5 * fov)
x2 = ee_val[0] + max_wall * np.cos(ee_orientation - 0.5 * fov)
y2 = ee_val[1] + max_wall * np.sin(ee_orientation - 0.5 * fov)
vis_tri = Polygon([tuple(ee_val), (x1, y1), (x2,y2)])
# define observer
if is_in_bounds(ee_val):
observer = vis.Point(float(ee_val[0]), float(ee_val[1]))
observer.snap_to_boundary_of(vis_env, eps)
observer.snap_to_vertices_of(vis_env, eps)
isovist = vis.Visibility_Polygon(observer, vis_env, eps)
# get environment in points
point_x , point_y = save_print(isovist)
if len(point_x ) == 0 or len(point_y) == 0:
continue
point_x.append(isovist[0].x())
point_y.append(isovist[0].y())
poly = Polygon([(x,y) for (x,y) in zip(point_x, point_y)])
visilbe_poly = poly.intersection(vis_tri)
if type(visilbe_poly) == Polygon and len(list(visilbe_poly.exterior.coords)) > 0:
visilbe_poly_pts = np.array(list(visilbe_poly.exterior.coords)).reshape((-1,1,2)).astype(int)
# draw visilbe polygon of the observer
cv2.fillPoly(img_copy, [visilbe_poly_pts], 150)
# draw obstacles and inspection points
obstacles.apply(lambda x: cv2.rectangle(img_copy, (x['x'], x['y']), (x['x']+x['width'], x['y']+x['length']), 255, -1), axis=1)
inspection_points.apply(lambda x: cv2.rectangle(img_copy, (x['x'], x['y']), (x['x'], x['y']), 100, -1), axis=1)
# add sample (x,y)
cv2.imwrite(f'sample_path_{trial_idx}_{p_zb}_{j}.png', img_copy)
def testVisilibity():
# Define an epsilon value (should be != 0.0)
epsilon = 1e-7
# Define the points which will be the outer boundary of the environment
# Must be COUNTER-CLOCK-WISE(ccw)
p1 = vis.Point(0, 0)
p2 = vis.Point(700, 0)
p3 = vis.Point(700, 900)
p4 = vis.Point(0, 900)
# Load the values of the outer boundary polygon in order to draw it later
wall_x = [p1.x(), p2.x(), p3.x(), p4.x(), p1.x()]
wall_y = [p1.y(), p2.y(), p3.y(), p4.y(), p1.y()]
# Outer boundary polygon must be COUNTER-CLOCK-WISE(ccw)
# Create the outer boundary polygon
walls = vis.Polygon([p1, p2, p3, p4])
# Define the point of the "observer"
observer = vis.Point(235, 400)
# Uncomment the following line in order to create a cone polygon
#walls = create_cone((observer.x(), observer.y()), 500, 270, 30, quality= 3)
# Walls should be in standard form
print('Walls in standard form : ', walls.is_in_standard_form())
# Now we define some holes for our environment. The holes must be inside
# our outer boundary polygon. A hole blocks the observer vision, it works as
# an obstacle in his vision sensor.
# We define some point for a hole. You can add more points in order to get
# the shape you want.
# The smalles point should be first
p2 = vis.Point(100, 300)
p3 = vis.Point(100, 500)
p4 = vis.Point(150, 500)
p1 = vis.Point(150, 300)
# Load the values of the hole polygon in order to draw it later
hole_x = [p1.x(), p2.x(), p3.x(), p4.x(), p1.x()]
hole_y = [p1.y(), p2.y(), p3.y(), p4.y(), p1.y()]
# Note: The point of a hole must be in CLOCK-WISE(cw) order.
# Create the hole polygon
hole = vis.Polygon([p2, p3, p4, p1])
# Check if the hole is in standard form
print('Hole in standard form: ', hole.is_in_standard_form())
# Define another point of a hole polygon
# Remember: the list of points must be CLOCK-WISE(cw)
p1 = vis.Point(300, 300)
p2 = vis.Point(300, 500)
p3 = vis.Point(400, 550)
p4 = vis.Point(400, 300)
# Load the values of the hole polygon in order to draw it later
hole1_x = [p1.x(), p2.x(), p3.x(), p4.x(), p1.x()]
hole1_y = [p1.y(), p2.y(), p3.y(), p4.y(), p1.y()]
# Create the hole polygon
hole1 = vis.Polygon([p1, p2, p3, p4])
# Check if the hole is in standard form
print('Hole in standard form: ', hole1.is_in_standard_form())
# Define another point of a hole polygon
# Remember: the list of points must be CLOCK-WISE(cw)
p2 = vis.Point(90, 700)
p3 = vis.Point(250, 750)
p4 = vis.Point(220, 600)
p1 = vis.Point(150, 600)
# Load the values of the hole polygon in order to draw it later
hole2_x = [p1.x(), p2.x(), p3.x(), p4.x(), p1.x()]
hole2_y = [p1.y(), p2.y(), p3.y(), p4.y(), p1.y()]
# Create the hole polygon
hole2 = vis.Polygon([p2, p3, p4, p1])
# Check if the hole is in standard form
print('Hole in standard form: ', hole2.is_in_standard_form())
# Define another point of a hole polygon
# Remember: the list of points must be CLOCK-WISE(cw)
p1 = vis.Point(330, 700)
p2 = vis.Point(330, 800)
p3 = vis.Point(530, 850)
p4 = vis.Point(530, 790)
# Load the values of the hole polygon in order to draw it later
hole3_x = [p1.x(), p2.x(), p3.x(), p4.x(), p1.x()]
hole3_y = [p1.y(), p2.y(), p3.y(), p4.y(), p1.y()]
# Create the hole polygon
hole3 = vis.Polygon([p1, p2, p3, p4])
# Check if the hole is in standard form
print('Hole in standard form: ', hole3.is_in_standard_form())
# Define another point of a hole polygon
# Remember: the list of points must be CLOCK-WISE(cw)
p1 = vis.Point(230, 50)
p2 = vis.Point(250, 90)
p3 = vis.Point(390, 90)
p4 = vis.Point(390, 50)
# Load the values of the hole polygon in order to draw it later
hole4_x = [p1.x(), p2.x(), p3.x(), p4.x(), p1.x()]
hole4_y = [p1.y(), p2.y(), p3.y(), p4.y(), p1.y()]
# Create the hole polygon
hole4 = vis.Polygon([p1, p2, p3, p4])
# Check if the hole is in standard form
print('Hole in standard form: ', hole4.is_in_standard_form())
# Create environment, wall will be the outer boundary because
# is the first polygon in the list. The other polygons will be holes
env = vis.Environment([walls, hole, hole2, hole1, hole3, hole4])
# Check if the environment is valid
print('Environment is valid : ', env.is_valid(epsilon))
# Define another point, could be used to check if the observer see it, to
# check the shortest path from one point to the other, etc.
end = vis.Point(330, 525)
# Define another point that the 'observer' will see
end_visible = vis.Point(415, 45)
# Necesary to generate the visibility polygon
observer.snap_to_boundary_of(env, epsilon)
observer.snap_to_vertices_of(env, epsilon)
# Obtein the visibility polygon of the 'observer' in the environmente
# previously define
isovist = vis.Visibility_Polygon(observer, env, epsilon)
# Uncomment the following line to obtein the visibility polygon
# of 'end' in the environmente previously define
#polygon_vis = vis.Visibility_Polygon(end, env, epsilon)
# Obtein the shortest path from 'observer' to 'end' and 'end_visible'
# in the environment previously define
shortest_path = env.shortest_path(observer, end, epsilon)
shortest_path1 = env.shortest_path(observer, end_visible, epsilon)
# Print the length of the path
print("Shortest Path length from observer to end: ",
shortest_path.length())
print("Shortest Path length from observer to end_visible: ",
shortest_path1.length())
logger.debug('shortest_path:\n%s', dir(shortest_path))
logger.debug('shortest_path.set_vertices:\n%s', shortest_path.size())
# logger.debug('shortest_path.path:\n%s',shortest_path)
# Check if 'observer' can see 'end', i.e., check if 'end' point is in
# the visibility polygon of 'observer'
print("Can observer see end? ", end._in(isovist, epsilon))
print("Can observer see end_visible? ", end_visible._in(isovist, epsilon))
# Print the point of the visibility polygon of 'observer' and save them
# in two arrays in order to draw the polygon later
logger.debug('isovist: %s', isovist.n())
return
point_x, point_y = save_print(isovist)
# Add the first point again because the function to draw, draw a line from
# one point to the next one and to close the figure we need the last line
# from the last point to the first one
point_x.append(isovist[0].x())
point_y.append(isovist[0].y())
# Set the title
p.title('VisiLibity Test')
# Set the labels for the axis
p.xlabel('X Position')
p.ylabel('Y Position')
# Plot the outer boundary with black color
p.plot(wall_x, wall_y, 'black')
# Plot the position of the observer with a green dot ('go')
p.plot([observer.x()], [observer.y()], 'go')
# Plot the position of 'end' with a green dot ('go')
p.plot([end.x()], [end.y()], 'go')
# Plot the position of 'end_visible' with a green dot ('go')
p.plot([end_visible.x()], [end_visible.y()], 'go')
# Plot the visibility polygon of 'observer'
p.plot(point_x, point_y)
# Plot the hole polygon with red color
p.plot(hole_x, hole_y, 'r')
# Plot the hole polygon with red color
p.plot(hole1_x, hole1_y, 'r')
# Plot the hole polygon with red color
p.plot(hole2_x, hole2_y, 'r')
# Plot the hole polygon with red color
p.plot(hole3_x, hole3_y, 'r')
# Plot the hole polygon with red color
p.plot(hole4_x, hole4_y, 'r')
# Example of a cone-shape polygon
cone_point = vis.Point(440, 420)
# cone=create_cone([cone_point.x(),cone_point.y()],150,0,45,3)
# cone_x,cone_y=save_print(cone)
# cone_x.append(cone_x[0])
# cone_y.append(cone_y[0])
# p.plot([cone_point.x()],[cone_point.y()],'go')
# p.plot(cone_x,cone_y)
# Show the plot
p.show()
def calculate_solution():
# The Ilya Constant (TM)
epsilon = 0.000000001
# Environment plot (500 x 500 should be enough, right?! *gulps with fear*)
p1 = vis.Point(50, 50)
p2 = vis.Point(-50, 50)
p3 = vis.Point(-50, -50)
p4 = vis.Point(50, -50)
# Set up our walls (yay)
wall_x = [p1.x(), p2.x(), p3.x(), p4.x(), p1.x()]
wall_y = [p1.y(), p2.y(), p3.y(), p4.y(), p1.y()]
walls = vis.Polygon([p1, p2, p3, p4])
#robot1 = vis.Point(-1,-1)
#robot2 = vis.Point(4,4)
#obstacle = vis.Polygon([vis.Point(1,6),vis.Point(1,1),vis.Point(5,1),vis.Point(5,5),vis.Point(3,5),vis.Point(3,3),vis.Point(4,3),vis.Point(4,2),
# vis.Point(2,2),vis.Point(2,6),vis.Point(6,6),vis.Point(6,0),vis.Point(0,0),vis.Point(0,6),vis.Point(1,6)][::-1])
#obstacle_x = [1, 1, 5, 5, 3, 3, 4, 4, 2, 2, 6, 6, 0, 0, 1][::-1]
#obstacle_y = [6, 1, 1, 5, 5, 3, 3, 2, 2, 6, 6, 0, 0, 6, 6][::-1]
robot1 = vis.Point(0, 1)
robot2 = vis.Point(6, 2)
obstacle = vis.Polygon([vis.Point(8,1),vis.Point(4,1),vis.Point(4,4),vis.Point(5,2), vis.Point(8,1)])
obstacle_x = [8, 4, 4, 5, 8]
obstacle_y = [1, 1, 4, 2, 1]
obstacle2 = vis.Polygon([vis.Point(1,2), vis.Point(1,4), vis.Point(3,4), vis.Point(3,2), vis.Point(1,2)][::-1])
obstacle2_x = [1, 1, 3, 3, 1][::-1]
obstacle2_y = [2, 4, 4, 2, 2][::-1]
env = vis.Environment([walls, obstacle, obstacle2])
robot1.snap_to_boundary_of(env, epsilon)
robot1.snap_to_vertices_of(env, epsilon)
isovist = vis.Visibility_Polygon(robot1, env, epsilon)
shortest_path = env.shortest_path(robot1, robot2, epsilon)
point_x, point_y = save_print(isovist)
point_x.append(isovist[0].x())
point_y.append(isovist[0].y())
p.title('Shortest (????) Path')
p.xlabel('X Position')
p.ylabel('Y Position')
p.plot(wall_x, wall_y, 'black')
p.plot([robot1.x()], [robot1.y()], 'go')
p.plot([robot2.x()], [robot2.y()], 'go')
p.plot(obstacle_x, obstacle_y, 'r')
p.plot(obstacle2_x, obstacle2_y, 'r')
print "Shortest Path length from observer to end: ", shortest_path.length()
print "Number of options: ", shortest_path.size()
polyline = []
for x in range(0, shortest_path.size()):
point = shortest_path.getVertex(x)
print "(", point.x(), ", ", point.y(), ")"
polyline.append(point)
p.show()
def make_arena_polygon():
#build center diamond polygon
#list points cw
p1 = vis.Point(23, ym - 16)
p2 = vis.Point(22, ym - 15)
p3 = vis.Point(22, ym - 13)
p4 = vis.Point(23, ym - 12)
p5 = vis.Point(25, ym - 12)
p6 = vis.Point(26, ym - 13)
p7 = vis.Point(26, ym - 15)
p8 = vis.Point(25, ym - 16)
# Load the values of the hole polygon in order to draw it later
diam_x = [
p2.x(),
p3.x(),
p4.x(),
p5.x(),
p6.x(),
p7.x(),
p8.x(),
p1.x(),
p2.x()
]
diam_y = [
p2.y(),
p3.y(),
p4.y(),
p5.y(),
p6.y(),
p7.y(),
p8.y(),
p1.y(),
p2.y()
]
# Create the hole polygon
diam = vis.Polygon([p2, p3, p4, p5, p6, p7, p8, p1])
#build the arena
wall_x, wall_y, walls = outer(
2, 28, 1, 47) #y_up, y_down, x_left, x_right, use arena format
#format y_up, y_down, x_left, x_right, use arena format
huecos_input = [
[ym - 6, ym - 7, 1, 7], #
[ym - 12, ym - 14, 10, 14], #
[ym - 20, ym - 26, 9, 11], #
[ym - 5, ym - 7, 21, 27], #
[ym - 19, ym - 21, 21, 27], #
[ym - 12, ym - 14, 34, 38],
[ym - 19, ym - 20, 41, 47], #
[ym, ym - 6, 37, 39], #
]
huecos = []
for item in huecos_input:
huecos.append(hole(item[0], item[1], item[2], item[3]))
# Create environment, wall will be the outer boundary because
# is the first polygon in the list. The other polygons will be holes
arena_poly = vis.Environment([
walls, huecos[0][2], huecos[1][2], huecos[2][2], huecos[3][2],
huecos[4][2], huecos[5][2], huecos[6][2], huecos[7][2], diam
])
return arena_poly, [[wall_x, wall_y], [huecos[0][0], huecos[0][1]],
[huecos[1][0], huecos[1][1]],
[huecos[2][0], huecos[2][1]],
[huecos[3][0], huecos[3][1]],
[huecos[4][0], huecos[4][1]],
[huecos[5][0], huecos[5][1]],
[huecos[6][0], huecos[6][1]],
[huecos[7][0], huecos[7][1]], [diam_x, diam_y]]
def __get_isovist(self, p):
v = vis.Point(*p)
v.snap_to_boundary_of(self.env, self.eps)
v.snap_to_vertices_of(self.env, self.eps)
return vis.Visibility_Polygon(v, self.env, self.eps)
def point_to_vis(point):
x,y = point
return vis.Point(x,y)
def points_to_vis_points(tupled_points):
return [vis.Point(x,y) for x,y in tupled_points]
def find_cut_space(P, v):
"""
Generate the cut space at v using Visilibity library.
"""
epsilon = 0.0000001
# Using shapely library, compute the cone of bisection
# shp_polygon = shapely.geometry.polygon.orient(Polygon(*P))
# shp_cone = shapely.geometry.polygon.orient(Polygon(find_cone_of_bisection(P, v)))
shp_polygon = Polygon(*P)
#print("Polygon: %s"%shp_polygon)
cone_of_bisection = find_cone_of_bisection(P, v)
shp_cone = Polygon(find_cone_of_bisection(P, v))
#print("Cone of bisection: %s"%cone_of_bisection)
shp_intersection = shp_cone.intersection(shp_polygon)
#print("Intersection: %s"%shp_intersection)
# import pylab as p
#
# # Plot the polygon itself
# x, y = shp_polygon.exterior.xy
# p.plot(x, y)
# x, y = shp_cone.exterior.xy
# p.plot(x, y)
# x, y = shp_intersection.exterior.xy
# p.plot(x, y)
# p.show()
# plot the intersection of the cone with the polygon
#intersection_x, intersection_y = shp_intersection.exterior.xy
#p.plot(intersection_x, intersection_y)
#for interior in shp_intersection.interiors:
# interior_x, interior_y = interior.xy
# p.plot(interior_x, interior_y)
# Plot the reflex vertex
#p.plot([observer.x()], [observer.y()], 'go')
#p.plot(point_x, point_y)
if shp_intersection.geom_type == "MultiPolygon":
shp_intersection = shp_intersection[0]
#print shp_intersection
elif shp_intersection.geom_type == "GeometryCollection":
for shape in shp_intersection:
if shape.geom_type == "Polygon":
shp_intersection = shape
break
else:
#shp_intersection = shapely.geometry.polygon.orient(shp_intersection)
shp_intersection = (shp_intersection)
#shp_intersection = shapely.geometry.polygon.orient(shp_cone.intersection(shp_polygon))
#Using the visilibity library, define the reflex vertex
observer = vis.Point(*v)
# Define the walls of intersection in Visilibity domain
# To put into standard form, do this
exterior_coords = shp_intersection.exterior.coords[:-1]
x_min_idx, x_min = min(enumerate(exterior_coords), key=itemgetter(1))
exterior_coords = exterior_coords[7:]+exterior_coords[:7]
vis_intersection_wall_points = []
for point in exterior_coords:
vis_intersection_wall_points.append(vis.Point(*point))
#print 'Walls in standard form : ',vis.Polygon(vis_intersection_wall_points).is_in_standard_form()
#for i in range(len(vis_intersection_wall_points)):
#print vis_intersection_wall_points[i].x(), vis_intersection_wall_points[i].y()
#print point.x(), point.y()
# Define the holes of intersection in Visilibity domain
vis_intersection_holes = []
for interior in shp_intersection.interiors:
vis_intersection_hole_points = []
for point in list(interior.coords):
vis_intersection_hole_points.append(vis.Point(*point))
vis_intersection_holes.append(vis_intersection_hole_points)
#print 'Hole in standard form : ',vis.Polygon(vis_intersection_hole_points).is_in_standard_form()
# Construct a convinient list
env = []
env.append(vis.Polygon(vis_intersection_wall_points))
for hole in vis_intersection_holes:
env.append(vis.Polygon(hole))
# Construct the whole envrionemt in Visilibity domain
env = vis.Environment(env)
# Construct the visible polygon
observer.snap_to_boundary_of(env, epsilon)
observer.snap_to_vertices_of(env, epsilon)
vis_free_space = vis.Visibility_Polygon(observer, env, epsilon)
#print vis_free_space.n()
def save_print(polygon):
end_pos_x = []
end_pos_y = []
for i in range(polygon.n()):
x = polygon[i].x()
y = polygon[i].y()
end_pos_x.append(x)
end_pos_y.append(y)
return end_pos_x, end_pos_y
point_x , point_y = save_print(vis_free_space)
point_x.append(vis_free_space[0].x())
point_y.append(vis_free_space[0].y())
###
# At this point, we have visibility polygon.
# Now we need to find edges of visbility polygon which are on the boundary
shp_visib = shapely.geometry.polygon.orient(Polygon(zip(point_x, point_y)),-1)
shp_ls_visib = LineString(shp_visib.exterior.coords[:])
shp_pl_visib = shp_ls_visib.buffer(0.001)
shp_ls_exterior = LineString(shp_polygon.exterior)
shp_ls_interior = []
for interior in shp_polygon.interiors:
shp_ls_interior.append(LineString(interior))
# Start adding cut space on the exterior
cut_space = []
common_items = []
#common_items = shp_ls_exterior.intersection(shp_ls_visib)
common_items = shp_ls_exterior.intersection(shp_pl_visib)
# Filter out very small segments
if common_items.geom_type == "MultiLineString":
for item in common_items:
linestring = item.coords[:]
# Examine each edge of the linestring
for i in range(len(linestring)-1):
edge = linestring[i:i+2]
edge_ls = LineString(edge)
if edge_ls.length > 0.02:
cut_space.append(edge)
elif common_items.geom_type == "LineString":
# Examine each edge of the linestring
linestring = common_items.coords[:]
for i in range(len(linestring)-1):
edge = linestring[i:i+2]
edge_ls = LineString(edge)
if edge_ls.length > 0.02:
cut_space.append(edge)
#print cut_space
# Start adding cut space on the holes
for interior in shp_polygon.interiors:
common_items = interior.intersection(shp_ls_visib)
if common_items.geom_type == "GeometryCollection":
# print common_items
for item in common_items:
if item.geom_type == "LineString":
cut_space.append(item.coords[:])
elif common_items.geom_type == "LineString":
cut_space.append(common_items.coords[:])
#Point, LineString, GeometryCollection
#print cut_space
# PLOTTING
# import pylab as p
#
# # Plot the polygon itself
# x, y = shp_polygon.exterior.xy
# p.plot(x, y)
#
# # plot the intersection of the cone with the polygon
# intersection_x, intersection_y = shp_intersection.exterior.xy
# p.plot(intersection_x, intersection_y)
#
# #for interior in shp_intersection.interiors:
# # interior_x, interior_y = interior.xy
# # p.plot(interior_x, interior_y)
#
# # Plot the reflex vertex
# p.plot([observer.x()], [observer.y()], 'go')
#
# p.plot(point_x, point_y)
#
# p.show()
#print cut_space
return cut_space
def a_point(x, y): #input arena coordinates
return vis.Point(x, y_size - (y - 1))
