def main(): start_time = time.time() start_node = node_rrt.Node_rrt(current_coords=(-5001.787385427393, 9526.705177228898), parent_coords=None, distance=0) goal_node = node_rrt.Node_rrt(current_coords=(-2833, 4969), parent_coords=None, distance=0) fig, ax = plt.subplots() ax.set_xlim(-6000, -1000) ax.set_ylim(4000, 10000) fig.gca().set_aspect('equal', adjustable='box') utils.plotPoint(start_node.getXYCoords(), ax, radius=0.06, color='cyan') # start utils.plotPoint(goal_node.getXYCoords(), ax, radius=0.06, color='magenta') # end obs.generateMap(ax) plt.ion() rrt_path, _, itr = rrtPlannedPath(start_node, goal_node, STEP_SIZE, plotter=ax, write=False) if rrt_path is not None: utils.plotPath(rrt_path, plotter=ax) finish_time = time.time() result = finish_time - start_time print("Program time: " + str(result) + " seconds.") plt.ioff() plt.show() rrt_path_coords = utils.convertNodeList2CoordList(node_list=rrt_path) with open('/home/valeriia/UAV_Swarm_gazebo/catkin_ws/src/coverage_planner/scripts/path_rrt_start_test.txt', 'w') as fp: fp.write('\n'.join('%s %s' % x for x in rrt_path_coords))
def main(): start_node = node.Node(current_coords=(-4, -4), parent_coords=None, distance=0) goal_node = node.Node(current_coords=(4, 4), parent_coords=None, distance=0) fig, ax = plt.subplots() ax.set_xlim(-6, 6) ax.set_ylim(-6, 6) fig.gca().set_aspect('equal', adjustable='box') utils.plotPoint(start_node.getXYCoords(), ax, radius=0.06, color='cyan') # start utils.plotPoint(goal_node.getXYCoords(), ax, radius=0.06, color='magenta') # end obs.generateMap(ax) plt.ion() rrt_path, _, itr = rrtPlannedPath(start_node, goal_node, robot_radius=DRONE_RADIUS, plotter=ax, write=False) if rrt_path is not None: utils.plotPath(rrt_path, plotter=ax) plt.ioff() plt.show()
def initPlot(start_xy, goal_xy, title=""): """ Initializes the plot. :param start_xy: The start xy node :type start_xy: tuple :param goal_xy: The goal xy node :type goal_xy: tuple :param title: frame title :type title: string :returns: Plotter :rtype: matplotlib.pyplot """ fig, ax = plt.subplots() fig.suptitle(title, fontsize=16) ax.set(xlim=(-5, 5), ylim=(-5, 5)) ax.set_aspect('equal') obstacles.generateMap(plotter=ax) markNodeXY(start_xy, plotter=ax, color='#00FF00', marker='o') markNodeXY(goal_xy, plotter=ax, color='#FF0000', marker='^') return ax
def visualizeNewPath(BC_x, BC_y, point_list, animate=False, write_path=None, itr=-1): fig, ax = plt.subplots() fig.gca().set_aspect('equal', adjustable='box') ax.set_xlim(-6, 6) ax.set_ylim(-6, 6) obs.generateMap(plotter=ax) utils.plotPoints(point_list, ax, radius=0.04, color='black') # all points utils.plotPoint(point_list[0], ax, radius=0.06, color='cyan') # start utils.plotPoint(point_list[-1], ax, radius=0.06, color='magenta') # end utils.plotPath(path=point_list, plotter=ax, path_color='black') utils.plotPath(path=zip(BC_x, BC_y), plotter=ax, path_color='lime') if write_path is not None: if itr > -1: plt.savefig(os.path.join(write_path, ('%04d.png' % itr))) itr += 1 return itr else: plt.savefig(os.path.join(write_path, ('frame.png'))) plt.show() if animate: plt.pause(0.5)
def main(): json_data = [] with open('Map2.json', 'r') as f: json_data = json.load(f) reset_point_ = json_data.get('reset_point') landing_point_ = json_data.get('landing_point') reset_point = reset_point_.values() landing_point = landing_point_.values() start_time = time.time() start_node = node_rrt.Node_rrt(current_coords=(reset_point[0], reset_point[1]), parent_coords=None, distance=0) goal_node = node_rrt.Node_rrt(current_coords=(landing_point[0],landing_point[1] ), parent_coords=None, distance=0) fig, ax = plt.subplots() ax.set_xlim(-15000, 15000) ax.set_ylim(-15000, 15000) fig.gca().set_aspect('equal', adjustable='box') utils.plotPoint(start_node.getXYCoords(), ax, radius=0.06, color='cyan') # start utils.plotPoint(goal_node.getXYCoords(), ax, radius=0.06, color='magenta') # end obs.generateMap(ax) plt.ion() rrt_path, _, itr = rrtPlannedPath(start_node, goal_node, STEP_SIZE, plotter=ax, write=False) if rrt_path is not None: utils.plotPath(rrt_path, plotter=ax) finish_time = time.time() result = finish_time - start_time print("Program time: " + str(result) + " seconds.") plt.ioff() path_co = np.array(utils.convertNodeList2CoordList(rrt_path)) rrt_prune_smooth_path_coords=path_pruning.prunedPath(path=path_co, radius=DRONE_RADIUS, clearance=(DRONE_RADIUS/2)) rrt_prune_smooth_path_coords= np.array(rrt_prune_smooth_path_coords[::-1]) plt.plot(rrt_prune_smooth_path_coords[:,0],rrt_prune_smooth_path_coords[:,1],'cyan') plt.show() rrt_path_coords = utils.convertNodeList2CoordList(node_list=rrt_path) with open('/home/valeriia/UAV_Swarm_gazebo/catkin_ws/src/coverage_planner/scripts/path_rrt_land_test.txt', 'w') as fp: fp.write('\n'.join('%s %s' % x for x in rrt_path_coords)) with open('/home/valeriia/UAV_Swarm_gazebo/catkin_ws/src/coverage_planner/scripts/path_rrt_land_test_smooth.txt', 'w') as fp: fp.write('\n'.join('%s %s' % x for x in list(map(tuple, rrt_prune_smooth_path_coords))))
def testMain(): fig, ax = plt.subplots() obstacles.generateMap(ax) ax.set_xlim(-6, 6) ax.set_ylim(-6, 6) fig.gca().set_aspect('equal', adjustable='box') # path = np.load('./path.npy', allow_pickle=True) # print(len(path)) # path_new = [[-4,-4],[-2, -4], [0,-4], [0, -3]] path_new = np.array([[-4., -4.], [-3.79264131, -3.6579439], [-3.46906898, -3.42278236], [-3.48912813, -3.02328564], [-3.3948723, -2.63454943], [-3.1296557, -2.33511637], [-2.89075655, -2.01429369], [-2.7065285, -1.65924436], [-2.36537512, -1.45040382], [-2.07738126, -1.17280782], [-1.83184281, -0.85703754], [-2.13718564, -0.59864683], [-2.25697259, -0.21700421], [-2.17639411, 0.17479562], [-1.89260448, 0.45668824], [-1.71137551, 0.81327772], [-1.40609844, 1.07174612], [-1.00910694, 1.12071291], [-0.63795093, 1.26985452], [-0.30719887, 1.49480579], [-0.35420504, 1.89203421], [0.02984229, 2.0038718], [0.42573924, 1.94672632], [0.47830175, 1.55019488], [0.6912239, 1.21157391], [1.07448352, 1.097066], [1.47007429, 1.03783814], [1.86993423, 1.02725365], [2.22281226, 1.2156073], [2.43382091, 1.55542393], [2.76428396, 1.3300483], [3.15622053, 1.40995904], [3.22079987, 1.80471151], [3.27615393, 2.2008629], [3.37145145, 2.58934505], [3.63297828, 2.89200612], [3.7511519, 3.27415137], [3.50783803, 3.59163894], [4., 4.]]) # path_new = [[-4,-3],[-3,-4], [-2,-4.5], [-0.5,-4], [0,-2],[2,0]] pr_path = prunedPath(path=path_new) pr_path = np.array(pr_path) plt.plot(path_new[:, 0], path_new[:, 1], 'g') plt.plot(pr_path[:, 0], pr_path[:, 1], 'r') plt.show()
def main(): json_data = [] with open('Map2.json', 'r') as f: json_data = json.load(f) start_point = json_data.get('start_point') print(start_point.values()) # find nearest point uav_location = start_point.values() with open( "/home/valeriia/UAV_Swarm_gazebo/catkin_ws/src/coverage_planner/scripts/path_cpp.txt", "r") as ins: path_cpp = [] for line in ins: path_cpp.append([float(line) for line in line.split() ]) # here send a list path_cpp samples = np.array(path_cpp) samples.sort(kind='quicksort') tree = KDTree(samples) uav_location_ = np.array([uav_location]) #here pud a goal pos after rrt closest_dist, closest_id = tree.query( uav_location_, k=1) # closest point with respect to the uav location point_of_entery = samples[closest_id] new_path_cpp = path_cpp[int(closest_id):] new_path_cpp.extend(path_cpp[:int(closest_id)]) del path_cpp[0:int(closest_id)] path_cpp_of_tuples = [tuple(l) for l in path_cpp] print(point_of_entery) #print(int(closest_id)) with open( '/home/valeriia/UAV_Swarm_gazebo/catkin_ws/src/coverage_planner/scripts/path_cpp_.txt', 'w') as fp: fp.write('\n'.join('%s %s' % x for x in path_cpp_of_tuples)) # rrt path planning start_time = time.time() start_node = node_rrt.Node_rrt(current_coords=(uav_location[0], uav_location[1]), parent_coords=None, distance=0) goal_node = node_rrt.Node_rrt(current_coords=(point_of_entery[0, 0, 0], point_of_entery[0, 0, 1]), parent_coords=None, distance=0) fig, ax = plt.subplots() ax.set_xlim(-15000, 15000) ax.set_ylim(-15000, 15000) fig.gca().set_aspect('equal', adjustable='box') utils.plotPoint(start_node.getXYCoords(), ax, radius=0.06, color='cyan') # start utils.plotPoint(goal_node.getXYCoords(), ax, radius=0.06, color='magenta') # end obs.generateMap(ax) plt.ion() rrt_path, _, itr = rrtPlannedPath(start_node, goal_node, STEP_SIZE, plotter=ax, write=False) if rrt_path is not None: utils.plotPath(rrt_path, plotter=ax) finish_time = time.time() result = finish_time - start_time print("Program time: " + str(result) + " seconds.") plt.ioff() path_co = np.array(utils.convertNodeList2CoordList(rrt_path)) rrt_prune_smooth_path_coords = path_pruning.prunedPath( path=path_co, radius=DRONE_RADIUS, clearance=(DRONE_RADIUS / 2)) rrt_prune_smooth_path_coords = np.array(rrt_prune_smooth_path_coords[::-1]) plt.plot(rrt_prune_smooth_path_coords[:, 0], rrt_prune_smooth_path_coords[:, 1], 'cyan') plt.show() rrt_path_coords = utils.convertNodeList2CoordList(node_list=rrt_path) #print(rrt_path_coords) with open( '/home/valeriia/UAV_Swarm_gazebo/catkin_ws/src/coverage_planner/scripts/path_rrt_start_test.txt', 'w') as fp: fp.write('\n'.join('%s %s' % x for x in rrt_path_coords)) with open( '/home/valeriia/UAV_Swarm_gazebo/catkin_ws/src/coverage_planner/scripts/path_rrt_start_test_smooth.txt', 'w') as fp: fp.write('\n'.join( '%s %s' % x for x in list(map(tuple, rrt_prune_smooth_path_coords))))
def main(): start_node = node.Node(current_coords=(-4, -4), parent_coords=None, distance=0) goal_node = node.Node(current_coords=(4, 4), parent_coords=None, distance=0) start1 = start_node.getXYCoords() quat = [0, 0, 0, 1] state_msg = ModelState() state_msg.model_name = 'iris' state_msg.pose.position.x = start1[0] state_msg.pose.position.y = start1[1] state_msg.pose.position.z = 0 state_msg.pose.orientation.x = quat[0] state_msg.pose.orientation.y = quat[1] state_msg.pose.orientation.z = quat[2] state_msg.pose.orientation.w = quat[3] set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) resp = set_state(state_msg) fig, ax = plt.subplots() ax.set_xlim(-6, 6) ax.set_ylim(-6, 6) fig.gca().set_aspect('equal', adjustable='box') utils.plotPoint(start_node.getXYCoords(), ax, radius=0.06, color='cyan') # start utils.plotPoint(goal_node.getXYCoords(), ax, radius=0.06, color='magenta') # end obs.generateMap(ax) plt.ion() rrt_path, _, itr = rrtPlannedPath(start_node, goal_node, robot_radius=DRONE_RADIUS, plotter=None, write=False) itr = 0 # plt.savefig(('./frames/%04d.png' % (itr))); itr += 1 # print(rrt_path) if rrt_path is not None: itr = utils.plotPath(rrt_path, plotter=ax, itr=itr, path_color='black') itr += 1 # plt.ioff() # plt.show() # Pruning path_co = np.array(utils.convertNodeList2CoordList(rrt_path)) print(path_co.shape) rrt_prune_smooth_path_coords = path_pruning.prunedPath( path=path_co, radius=DRONE_RADIUS, clearance=(DRONE_RADIUS / 2)) rrt_prune_smooth_path_coords = np.array(rrt_prune_smooth_path_coords[::-1]) # plt.plot(path_co[:,0],path_co[:,1],'green') # plt.plot(rrt_prune_smooth_path_coords[:,0],rrt_prune_smooth_path_coords[:,1],'cyan') # if path_co is not None: # utils.plotPath(path_co, plotter=ax, itr=itr, path_color='black') if rrt_prune_smooth_path_coords is not None: itr = utils.plotPath(rrt_prune_smooth_path_coords, plotter=ax, itr=itr, path_color='cyan') itr += 1 plt.ioff()
def main(): json_data = [] with open('Map2.json', 'r') as f: json_data = json.load(f) start_point_ = json_data.get('start_point') #centroid_ = json_data.get('centroid') '''json_data_2 = [] with open('Scenario2.json', 'r') as d: json_data_2 = json.load(d) target_point = json_data_2.get('centroid')''' x = [ -957.3915945077315, -910.8959478922188, -948.4347058273852, -875.1556231221184, -845.2041011163965, -858.6145041380078, -919.3042095946148, -942.3035844964907, -933.9325257679448, -889.955683006905 ] y = [ -855.0138749238104, -895.5183857586235, -921.3926183544099, -947.264425364323, -858.3795844987035, -861.4421726837754, -895.0775583777577, -844.3298955513164, -922.8892891705036, -887.7070486117154 ] centroid = [sum(x) / len(x), sum(y) / len(y)] print(centroid) start_point = start_point_.values() #target_point = centroid_.values() print(type(start_point)) print(centroid) start_time = time.time() start_node = node_rrt.Node_rrt(current_coords=(start_point[0], start_point[1]), parent_coords=None, distance=0) goal_node = node_rrt.Node_rrt(current_coords=(centroid[0], centroid[1]), parent_coords=None, distance=0) fig, ax = plt.subplots() ax.set_xlim(-15000, 15000) ax.set_ylim(-15000, 15000) fig.gca().set_aspect('equal', adjustable='box') utils.plotPoint(start_node.getXYCoords(), ax, radius=0.06, color='cyan') # start utils.plotPoint(goal_node.getXYCoords(), ax, radius=0.06, color='magenta') # end obs.generateMap(ax) plt.ion() rrt_path, _, itr = rrtPlannedPath(start_node, goal_node, STEP_SIZE, plotter=ax, write=False) if rrt_path is not None: utils.plotPath(rrt_path, plotter=ax) finish_time = time.time() result = finish_time - start_time print("Program time: " + str(result) + " seconds.") plt.ioff() path_co = np.array(utils.convertNodeList2CoordList(rrt_path)) rrt_prune_smooth_path_coords = path_pruning.prunedPath( path=path_co, radius=DRONE_RADIUS, clearance=(DRONE_RADIUS / 2)) rrt_prune_smooth_path_coords = np.array(rrt_prune_smooth_path_coords[::-1]) plt.plot(rrt_prune_smooth_path_coords[:, 0], rrt_prune_smooth_path_coords[:, 1], 'cyan') plt.show() rrt_path_coords = utils.convertNodeList2CoordList(node_list=rrt_path) with open( '/home/valeriia/UAV_Swarm_gazebo/catkin_ws/src/coverage_planner/scripts/path_rrt_test_scenario2.txt', 'w') as fp: fp.write('\n'.join('%s %s' % x for x in rrt_path_coords)) with open( '/home/valeriia/UAV_Swarm_gazebo/catkin_ws/src/coverage_planner/scripts/path_rrt_test__scenario2_smooth.txt', 'w') as dp: dp.write('\n'.join( '%s %s' % x for x in list(map(tuple, rrt_prune_smooth_path_coords))))
def a_star(start_rc, goal_rc, orientation, rpm1=10, rpm2=20, clearance=0.2, viz_please=False): """ A-star algorithm given the start and the goal nodes. :param start_rc: The start position :type start_rc: tuple :param goal_rc: The goal position :type goal_rc: tuple :param orientation: The orientation :type orientation: number :param rpm1: The rpm 1 :type rpm1: number :param rpm2: The rpm 2 :type rpm2: number :param clearance: The clearance :type clearance: number :param viz_please: Whether to visualize or not :type viz_please: boolean :returns: Returns path nodes and the list of visited nodes. :rtype: tuple """ """ Inputs """ start_node = node.Node(current_coords=start_rc, parent_coords=None, orientation=0, parent_orientation=None, action=None, movement_cost=0, goal_cost=utils.euclideanDistance( start_rc, goal_rc)) print("-----------------------") print("Start Node:") start_node.printNode() print("-----------------------") goal_node = node.Node(current_coords=goal_rc, parent_coords=None, orientation=None, parent_orientation=None, action=None, movement_cost=None, goal_cost=0) """ Initializations """ action_set = [(0, rpm1), (rpm1, 0), (0, rpm2), (rpm2, 0), (rpm1, rpm2), (rpm2, rpm1), (rpm1, rpm1), (rpm2, rpm2)] min_heap = [((start_node.movement_cost + start_node.goal_cost), start_node) ] heapq.heapify(min_heap) visited = {} visited.update({ (utils.getKey(start_rc[0], start_rc[1], start_node.orientation)): start_node }) visited_viz_nodes = [start_node] """ Initialize the plot figures if the visualization flag is true. Also mark the start and the goal nodes. """ if viz_please: fig, ax = plt.subplots() ax.set(xlim=(an.MIN_COORDS[0], an.MAX_COORDS[0]), ylim=(an.MIN_COORDS[1], an.MAX_COORDS[1])) # ax.set(xlim=(-5, 5), ylim=(-5, 5)) ax.set_aspect('equal') obstacles.generateMap(plotter=ax) viz.markNode(start_node, plotter=ax, color='#00FF00', marker='o') viz.markNode(goal_node, plotter=ax, color='#FF0000', marker='^') plt.ion() """ Run the loop for A-star algorithm till the min_heap queue contains no nodes. """ while (len(min_heap) > 0): _, curr_node = heapq.heappop(min_heap) # Consider all the action moves for all the selected nodes. for action in action_set: new_node = an.actionMove(current_node=curr_node, next_action=action, goal_position=goal_rc, clearance=clearance) # Check if all the nodes are valid or not. if (new_node is not None): """ Check if the current node is a goal node. """ if new_node.goal_cost < GOAL_REACH_THRESH: print("Reached Goal!") print("Final Node:") new_node.printNode() visited_viz_nodes.append(new_node) path = an.backtrack(new_node, visited) print("------------------------") if viz_please: viz.plot_curve(new_node, plotter=ax, color="red") viz.plotPath(path, rev=True, pause_time=0.5, plotter=ax, color="lime", linewidth=4) plt.ioff() plt.show() return (path, visited_viz_nodes) """ Mark node as visited, Append to min_heap queue, Update if already visited. """ node_key = (utils.getKey(new_node.current_coords[0], new_node.current_coords[1], new_node.orientation)) if node_key in visited: if new_node < visited[node_key]: visited[node_key] = new_node min_heap.append( ((new_node.movement_cost + new_node.goal_cost), new_node)) else: if viz_please: viz.plot_curve(new_node, plotter=ax, color="red") plt.show() plt.pause(0.001) visited.update({node_key: new_node}) min_heap.append( ((new_node.movement_cost + new_node.goal_cost), new_node)) visited_viz_nodes.append(new_node) # Heapify the min heap to update the minimum node in the list. heapq.heapify(min_heap)
def main(): json_data = [] if os.path.exists('Map2.json'): with open('Map2.json', 'r') as f: json_data = json.load(f) reset_point = json_data.get('reset_point') drop_location = reset_point.values() else: print('####!!!!There is no file with mission!!!!####') json_data_2 = [] x_arr = [] y_arr = [] if os.path.exists('Targets.json'): with open('Targets.json', 'r') as d: json_data_2 = json.load(d) target_point_arr = json_data_2.get('points') for i in range(len(target_point_arr)): target_point_arr_ = target_point_arr[i] x = target_point_arr_[0] y = target_point_arr_[1] x_arr.append(x) y_arr.append(y) centroid = [sum(x_arr) / len(x_arr), sum(y_arr) / len(y_arr)] else: print('####!!!!There is no file with targets!!!!####') start_time = time.time() start_node = node_rrt.Node_rrt(current_coords=(centroid[0], centroid[1]), parent_coords=None, distance=0) goal_node = node_rrt.Node_rrt(current_coords=(drop_location[0], drop_location[1]), parent_coords=None, distance=0) fig, ax = plt.subplots() ax.set_xlim(-15000, 15000) ax.set_ylim(-15000, 15000) fig.gca().set_aspect('equal', adjustable='box') utils.plotPoint(start_node.getXYCoords(), ax, radius=0.06, color='cyan') # start utils.plotPoint(goal_node.getXYCoords(), ax, radius=0.06, color='magenta') # end obs.generateMap(ax) plt.ion() # Set plotter=None here to disable animation rrt_path, _, itr = rrtPlannedPath(start_node, goal_node, STEP_SIZE, plotter=ax, write=False) if rrt_path is not None: utils.plotPath(rrt_path, plotter=ax) finish_time = time.time() result = finish_time - start_time print("Program time: " + str(result) + " seconds.") plt.ioff() path_co = np.array(utils.convertNodeList2CoordList(rrt_path)) rrt_prune_smooth_path_coords = path_pruning.prunedPath( path=path_co, radius=DRONE_RADIUS, clearance=(DRONE_RADIUS)) rrt_prune_smooth_path_coords = np.array(rrt_prune_smooth_path_coords[::-1]) plt.plot(rrt_prune_smooth_path_coords[:, 0], rrt_prune_smooth_path_coords[:, 1], 'cyan') plt.show() rrt_path_coords = utils.convertNodeList2CoordList(node_list=rrt_path) with open( '/home/valeriia/UAV_Swarm_gazebo/catkin_ws/src/px4_controller/rrt_pruning_smoothing/Code/path_rrt_reset_test_scenario2.txt', 'w') as fp: fp.write('\n'.join('%s %s' % x for x in rrt_path_coords)) with open( '/home/valeriia/UAV_Swarm_gazebo/catkin_ws/src/px4_controller/rrt_pruning_smoothing/Code/path_rrt_reset_test_smooth_scenario2.txt', 'w') as fp: fp.write('\n'.join( '%s %s' % x for x in list(map(tuple, rrt_prune_smooth_path_coords))))
def bezierCurveTesting1(point_list, animate=False, write_path=None): fig, ax = plt.subplots() fig.gca().set_aspect('equal', adjustable='box') ax.set_xlim(-6, 6) ax.set_ylim(-6, 6) obs.generateMap(plotter=ax) utils.plotPoints(point_list, ax, radius=0.04, color='black') # all points utils.plotPoint(point_list[0], ax, radius=0.06, color='cyan') # start utils.plotPoint(point_list[-1], ax, radius=0.06, color='magenta') # end if write_path is not None: write_itr = 0 plt.savefig(write_path + '/%s.png' % (str(write_itr))) write_itr += 1 if animate: plt.ion() # List of all path points BC_x = [] BC_y = [] # Initial points try: P0 = point_list[0] P1 = point_list[1] P2 = point_list[2] P3 = point_list[3] except Exception: print("Insufficient points. Appropriate case handled later.") # Starting Curve if len(point_list) > 4: bc_x, bc_y = bc.bezierCubicCurveEquation(P0, P1, P2, ((P2 + P3) / 2), step_size=0.01) elif len(point_list) == 4: bc_x, bc_y = bc.bezierCubicCurveEquation(P0, P1, P2, P3, step_size=0.01) elif len(point_list) == 3: bc_x, bc_y = bc.bezierQuadraticCurveEquation(P0, P1, P2, step_size=0.01) elif len(point_list) == 2: bc_x, bc_y = bc.bezierLinearCurveEquation(P0, P1, step_size=0.01) BC_x.extend(bc_x) BC_y.extend(bc_y) # Animation if animate: utils.plotPoints([P0, P1, P2, P3], ax, radius=0.04, color='red') plt.plot(bc_x, bc_y, color='green') plt.show() plt.pause(1) if write_path is not None: plt.savefig(write_path + '/%s.png' % (str(write_itr))) write_itr += 1 if animate: utils.plotPoints([P0, P1, P2, P3], ax, radius=0.04, color='yellow') ############## # Handling the rest of the points, leaving out the last 1 or 2 points for the end point_idx = 4 if point_idx >= len(point_list): plt.ioff() # plt.show() return (BC_x, BC_y) while point_idx + 2 < len(point_list): P0 = P2 P1 = P3 P2 = point_list[point_idx] point_idx += 1 P3 = point_list[point_idx] point_idx += 1 bc_x, bc_y = bc.bezierCubicCurveEquation(((P0 + P1) / 2), P1, P2, ((P2 + P3) / 2), 0.01) BC_x.extend(bc_x) BC_y.extend(bc_y) # Animation if animate: utils.plotPoints([P0, P1, P2, P3], ax, radius=0.04, color='red') plt.plot(bc_x, bc_y, color='green') plt.show() plt.pause(1) if write_path is not None: plt.savefig(write_path + '/%s.png' % (str(write_itr))) write_itr += 1 if animate: utils.plotPoints([P0, P1, P2, P3], ax, radius=0.04, color='yellow') ############## # Ending Curve P0 = P2 P1 = P3 P2 = point_list[point_idx] point_idx += 1 if point_idx < len(point_list): P3 = point_list[point_idx] bc_x, bc_y = bc.bezierCubicCurveEquation(((P0 + P1) / 2), P1, P2, P3, step_size=0.01) else: bc_x, bc_y = bc.bezierQuadraticCurveEquation(((P0 + P1) / 2), P1, P2, step_size=0.01) BC_x.extend(bc_x) BC_y.extend(bc_y) # Animation if animate: utils.plotPoints([P0, P1, P2, P3], ax, radius=0.04, color='red') plt.plot(bc_x, bc_y, color='green') plt.show() plt.pause(1) if write_path is not None: plt.savefig(write_path + '/%s.png' % (str(write_itr))) write_itr += 1 if animate: utils.plotPoints([P0, P1, P2, P3], ax, radius=0.04, color='yellow') ############## # Animation if animate: plt.ioff() else: plt.plot(BC_x, BC_y, color='green') # plt.show() if write_path is not None: plt.savefig(write_path + '/%s.png' % (str(write_itr))) ############## return (BC_x, BC_y)
def main(): prm.main() """ rospy.loginfo("This start x pose: %s", start_pose_x) rospy.loginfo("This start y pose: %s", start_pose_y) rospy.loginfo("This goal x pose: %s", goal_pose_x) rospy.loginfo("This goal y pose: %s", goal_pose_y)""" #possition_listener() if (prm.start_pose_x == None) or (prm.start_pose_y == None): rospy.loginfo("Problem with start coordinates!") rospy.loginfo("Start x: '%s'" + " and " + "y: '%s' coordinates: ", str(prm.start_pose_x), str(prm.start_pose_y)) sys.exit(0) elif (prm.goal_pose_x == None) or (prm.goal_pose_y == None): rospy.loginfo("Problem with goal coordinates!") rospy.loginfo("Goal x: '%s'" + " and " + "y: '%s' coordinates: ", str(prm.goal_pose_x), str(prm.goal_pose_y)) sys.exit(0) start_node = node_rrt.Node_rrt(current_coords=(prm.start_pose_x, prm.start_pose_y), parent_coords=None, distance=0) goal_node = node_rrt.Node_rrt(current_coords=(prm.goal_pose_x, prm.goal_pose_y), parent_coords=None, distance=0) start1 = start_node.getXYCoords() quat = [0, 0, 0, 1] state_msg = ModelState() state_msg.model_name = 'iris' state_msg.pose.position.x = start1[0] state_msg.pose.position.y = start1[1] state_msg.pose.position.z = 0 state_msg.pose.orientation.x = quat[0] state_msg.pose.orientation.y = quat[1] state_msg.pose.orientation.z = quat[2] state_msg.pose.orientation.w = quat[3] set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) resp = set_state(state_msg) fig, ax = plt.subplots() ax.set_xlim(prm._lim_x, prm.lim_x) ax.set_ylim(prm._lim_y, prm.lim_y) fig.gca().set_aspect('equal', adjustable='box') utils.plotPoint(start_node.getXYCoords(), ax, radius=0.06, color='cyan') # start utils.plotPoint(goal_node.getXYCoords(), ax, radius=0.06, color='magenta') # end obs.generateMap(ax) plt.ion() rrt_path, _, itr = rrtPlannedPath(start_node, goal_node, robot_radius=DRONE_RADIUS, plotter=None, write=False) itr = 0 # plt.savefig(('./frames/%04d.png' % (itr))); itr += 1 # print(rrt_path) if rrt_path is not None: itr = utils.plotPath(rrt_path, plotter=ax, itr=itr, path_color='black') itr += 1 # plt.ioff() # plt.show() # Pruning path_co = np.array(utils.convertNodeList2CoordList(rrt_path)) print(path_co.shape) rrt_prune_smooth_path_coords = path_pruning.prunedPath( path=path_co, radius=DRONE_RADIUS, clearance=(DRONE_RADIUS / 2)) rrt_prune_smooth_path_coords = np.array(rrt_prune_smooth_path_coords[::-1]) # plt.plot(path_co[:,0],path_co[:,1],'green') # plt.plot(rrt_prune_smooth_path_coords[:,0],rrt_prune_smooth_path_coords[:,1],'cyan') # if path_co is not None: # utils.plotPath(path_co, plotter=ax, itr=itr, path_color='black') if rrt_prune_smooth_path_coords is not None: itr = utils.plotPath(rrt_prune_smooth_path_coords, plotter=ax, itr=itr, path_color='cyan') itr += 1 #rrt_smoothed_path_coors = utils.convertNodeList2CoordList(node_list=rrt_prune_smooth_path_coords) #smoothed_path_length = utils.path_length_meters(rrt_smoothed_path_length) smoothed_path_pength = utils.path_length_meters( rrt_prune_smooth_path_coords) try: capacity = float(str(prm.capacity)[5:]) max_length = float(str(prm.max_length)[5:]) max_uav_path = capacity * max_length if max_uav_path < path_length: print("\nWarning!") print("Path cannot be overcomed!") else: print("Path can be overcomed!") except: print("Capacity and maximum path lenght was not entered!") with open('path.txt', 'w') as f: f.write(json.dumps(rrt_path_coords)) plt.ioff() plt.show() #plt.savefig(('./frames/%04d.png' % (itr))); itr += 1 rrt_path_coords = utils.convertNodeList2CoordList(node_list=rrt_path)
def main(): start_time = time.time() prm.main() print(X_LIM) #possition_listener() if (prm.start_pose_x == None) or (prm.start_pose_y == None): rospy.loginfo("Problem with start coordinates!") rospy.loginfo("Start x: '%s' and y: '%s' coordinates", str(prm.start_pose_x), str(prm.start_pose_y)) sys.exit(0) elif (prm.goal_pose_x == None) or (prm.goal_pose_y == None): rospy.loginfo("Problem with goal coordinates!") rospy.loginfo("Goal x: '%s' and y: '%s' coordinates", str(prm.goal_pose_x), str(prm.goal_pose_y)) sys.exit(0) start_node = node_rrt.Node_rrt(current_coords=(prm.start_pose_x, prm.start_pose_y), parent_coords=None, distance=0) goal_node = node_rrt.Node_rrt(current_coords=(prm.goal_pose_x, prm.goal_pose_y), parent_coords=None, distance=0) #start_node = node_rrt.Node_rrt(current_coords=(0, 0), parent_coords=None, distance=0) #goal_node = node_rrt.Node_rrt(current_coords=(5, 5), parent_coords=None, distance=0) fig, ax = plt.subplots() ax.set_xlim(prm._lim_x, prm.lim_x) ax.set_ylim(prm._lim_y, prm.lim_y) fig.gca().set_aspect('equal', adjustable='box') utils.plotPoint(start_node.getXYCoords(), ax, radius=0.06, color='cyan') # start utils.plotPoint(goal_node.getXYCoords(), ax, radius=0.06, color='magenta') # end #obs.testMain() obs.generateMap(ax) plt.ion() rrt_path, _, itr = rrtPlannedPath(start_node, goal_node, robot_radius=prm.c_drone_radius, plotter=ax, write=False) if rrt_path is not None: utils.plotPath(rrt_path, plotter=ax) rrt_path_coords = utils.convertNodeList2CoordList(node_list=rrt_path) path_length = utils.path_length_meters(rrt_path_coords) try: capacity = float(str(prm.capacity)[5:]) max_length = float(str(prm.max_length)[5:]) max_uav_path = capacity * max_length if max_uav_path < path_length: print("\nWarning!") print("Path cannot be overcomed!") else: print("Path can be overcomed!") except: print("Capacity and maximum path lenght was not entered!") finish_time = time.time() result = finish_time - start_time print("Program time: " + str(result) + " seconds.") #global rrt_path_coords_c #rrt_path_coords_c = [] #rrt_path_coords_c = utils.convertNodeList2CoordList(node_list=rrt_path) rrt_path_coords = utils.convertNodeList2CoordList(node_list=rrt_path) with open( '/home/valeriia/UAV_Swarm_gazebo/catkin_ws/src/coverage_planner/scripts/path_rrt_.txt', 'w') as fp: fp.write('\n'.join('%s %s' % x for x in rrt_path_coords)) #f = open('path.txt', 'w') #for element in rrt_path_coords: #line = ' '.join(str(x) for x in element) #f.write(line + '\n') #f.close() #with open('path.txt', 'w') as f: # for item in rrt_path_coords: # f.write(json.dumps(rrt_path_coords)) plt.ioff() plt.show() sys.exit