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 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 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 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 __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 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 make_robot_360(robot, env): rp = a_point(robot.x, 28 - robot.y) rp.snap_to_boundary_of(env, epsilon) rp.snap_to_vertices_of(env, epsilon) return vis.Visibility_Polygon(rp, env, epsilon)
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 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 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 __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 can_see(self,p1,p2): p1 = point_to_vis(p1) p2 = point_to_vis(p2) poly = vis.Visibility_Polygon(p1, self.environment, EPSILON) return p2._in(poly)
def get_visibilily_polygon(self,origin): origin_vis = point_to_vis(origin) isovist = vis.Visibility_Polygon(origin_vis, self.environment, EPSILON) pointlist = vis_poly_to_pointlist(isovist) return pointlist
def prepare_data(data_dir, dest_name, start_idx=0, scale=50, end_idx=100000, limit_samples=1e6, eps=1e-7): max_wall = 200 fov = np.pi / 2 imgs, c_points = [], [] counter_samples = 0 i = start_idx while (i < end_idx + 1 and limit_samples > 0 and counter_samples < limit_samples): i += 1 files_list = { 'obstacles': os.path.join(data_dir, f'test_planar_{i}_obstacles.csv'), 'inspection_points': os.path.join(data_dir, f'test_planar_{i}_inspection_points.csv'), 'configurations': os.path.join(data_dir, f'test_planar_{i}_conf'), 'vertex': os.path.join(data_dir, f'test_planar_{i}_vertex'), 'results': os.path.join(data_dir, f'test_search_{i}_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: continue # construct image with obstacles img = np.zeros((101, 101)) obstacles = (pd.read_csv(files_list['obstacles']) * scale).round(0).astype(int) #obstacles.apply(lambda x: cv2.rectangle(img, (x['x'], x['y']), (x['x']+x['width'], x['y']+x['length']), 255, -1), axis=1) 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: for line in f: vertices_idxs = [int(x) for x in line.split(' ')[1:-1]] img_copy = img.copy() for j in range(0, len(vertices_idxs) - 1): if (np.random.uniform() < 0.0): c_point = np.random.uniform(low=-np.pi, high=np.pi, size=5) if j > 0: j -= 1 else: 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) imgs.append(np.expand_dims(img_copy, axis=0)) c_points.append( np.expand_dims(cspace_df.iloc[j + 1].to_numpy(), axis=0)) print(f'processed images: {counter_samples}') counter_samples += 1 if i % 100 == 0: print(f'Processed solutions: {i}') # normilize data cs_np = np.concatenate(c_points) / np.pi imgs_np = np.concatenate(imgs).reshape([len(cs_np), -1]) / 255.0 # write h5 file to the same location with h5py.File(os.path.join(data_dir, dest_name), "w") as f: f.create_dataset("images", data=imgs_np, dtype=imgs_np.dtype) f.create_dataset("cpoints", data=cs_np, dtype=cs_np.dtype)
def compute(v, P): """ Compute the visible part of P from v """ #print("Previsible polygon: %s"%(P,)) # Used for visilibity library epsilon = 0.01 #Using the visilibity library, define the reflex vertex observer = vis.Point(*v[1]) # To put into standard form, do this if not LinearRing(P[0]).is_ccw: ext = P[0][::-1] else: ext = P[0] x_min_idx, x_min = min(enumerate(ext), key=lambda x: x[1]) ext = ext[x_min_idx:] + ext[:x_min_idx] #print x_min_idx, x_min # Define the walls of intersection in Visilibity domain wall_points = [] for point in ext: wall_points.append(vis.Point(*point)) #print 'Walls in standard form : ',vis.Polygon(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 holes = [] for interior in P[1]: hole_points = [] for point in interior: hole_points.append(vis.Point(*point)) holes.append(hole_points) #print 'Hole in standard form : ',vis.Polygon(hole_points).is_in_standard_form() # Construct a convinient list env = [] env.append(vis.Polygon(wall_points)) for hole in 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) 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()) return point_x, point_y
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 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 get_libvis_polygon(self,origin): origin_vis = point_to_vis(origin) return vis.Visibility_Polygon(origin_vis, self.environment, EPSILON)