def RNN(): # Stacking two columsn to the tree rrt.nodes = np.hstack( (rrt.nodes, np.arange(rrt.nodes.shape[0]).reshape(rrt.nodes.shape[0], 1))) found, min_cost_idx = rrt.check_goal() if found is True: path = rrt.find_path(min_cost_idx) else: print('path not found') path.reverse() rrt.nodes = rrt.nodes[0:rrt.nodes_num, :] replan = Replan() # starting point of the obstacle which is added after the RRT* is done obstacle_pose = Point(-5, 5) prev_obstacle_pose = obstacle_pose new_obstacle = Polygon(obstacle_pose.buffer(1 + 0.01)) i = -1 path_len = len(path) plot_no = 0 step = 0 while (i != path_len - 1): recheck_goal = False i += 1 node = path[i] rrt.p_current = node robot_pose = Point(rrt.nodes[int(path[i]), 0], rrt.nodes[int(path[i]), 1]) # if step == 0: # obstacle_pose = Point(-5,5) # prev_obstacle_pose = obstacle_pose # new_obstacle = Polygon(obstacle_pose.buffer(1+0.01)) # prev_obstacle_pose,path_len,recheck_goal = replan.moveObstacle(rrt,node,obstacle_pose,prev_obstacle_pose,path,i,step) # i = 0 if step == 3: # obstacle_pose = Point(7.5,2.5) # # prev_obstacle_pose = obstacle_pose # new_obstacle = Polygon(obstacle_pose.buffer(1+0.01)) # prev_obstacle_pose,path_len,recheck_goal = replan.moveObstacle(rrt,node,obstacle_pose,prev_obstacle_pose,path,i,step) # i = 0 # note whenever updating obstacle update rrt.obstacles as wwll rrt.nodes[rrt.nodes[:, 4] == 0, 4] = 1 obstacle_pose = Point(7.5, 2.5) prev_obstacle_pose = obstacle_pose new_obstacle = Polygon(obstacle_pose.buffer(1 + 0.01)) map_obstacles.addNewObstacle(new_obstacle) rrt.obstacles = map_obstacles.getObstacles() replan.prune_till_pcurr(node, rrt) path = path[i:] i = 0 path_len = len(path) replan.modify_tree_and_path(rrt, path) rrt.p_current = path[0] recheck_goal = True flag_obstacle = replan.detectObstacle(rrt, path, new_obstacle) if flag_obstacle is True: print('obstacle') rrt2 = RRT(rrt.obstacles) rrt2.p_current = 0 rrt2.start = rrt.nodes[rrt.p_current, 0:2] rrt2.nodes[0, 0:2] = rrt2.start rrt2.nodes[0][2] = 0 rrt2.nodes[0][3] = 0 j = 0 replan.prune_till_pcurr(rrt.p_current, rrt) replan.modify_tree_and_path(rrt, path) for i in range(6000): print(j) j = j + 1 x = rrt2.sample() nearest_index = rrt2.nearest(x) if nearest_index is None: continue x_new, cost_steer = rrt2.steer(x, rrt2.nodes[nearest_index][0:2]) check = rrt2.is_in_collision(x_new, rrt2.nodes[nearest_index][0:2]) if check is False: near_indices = rrt2.get_near_list(x_new) rrt2.connect(x_new, nearest_index, cost_steer, new=True) if near_indices.size != 0: best_index, cost = rrt2.nearest_from_list( x_new, near_indices, reconnect=False) if best_index is not (None): cost_steer = rrt2.get_dist( x_new, rrt2.nodes[best_index][0:2]) rrt2.connect(x_new, best_index, cost_steer, new=False) rrt2.rewire(x_new, near_indices) found, min_cost_idx = rrt2.check_goal() if found is True: path = rrt2.find_path(min_cost_idx) # plot(rrt2.nodes,robot_pose,None,0,path,True) break step = step + 1
def runRegrow(): # Stacking two columsn to the tree rrt.nodes = np.hstack( (rrt.nodes, np.arange(rrt.nodes.shape[0]).reshape(rrt.nodes.shape[0], 1))) found, min_cost_idx = rrt.check_goal() if found is True: path = rrt.find_path(min_cost_idx) else: print('path not found') path.reverse() rrt.nodes = rrt.nodes[0:rrt.nodes_num, :] replan = Replan() # starting point of the obstacle which is added after the RRT* is done obstacle_pose = Point(-5, 5) prev_obstacle_pose = obstacle_pose new_obstacle = Polygon(obstacle_pose.buffer(1 + 0.01)) i = -1 path_len = len(path) plot_no = 0 step = 0 blocked_tree_indices = replan.removeOccupiedNodes(rrt, path, i, new_obstacle) rrt.nodes[blocked_tree_indices, 4] = 0 while (i != path_len - 1): recheck_goal = False i += 1 print(plot_no) node = path[i] rrt.p_current = node robot_pose = Point(rrt.nodes[int(path[i]), 0], rrt.nodes[int(path[i]), 1]) if step == 3: rrt.nodes[rrt.nodes[:, 4] == 0, 4] = 1 obstacle_pose = Point(-5.0, -8.0) prev_obstacle_pose = obstacle_pose new_obstacle = Polygon(obstacle_pose.buffer(1 + 0.01)) map_obstacles.addNewObstacle(new_obstacle) rrt.obstacles = map_obstacles.getObstacles() replan.prune_till_pcurr(node, rrt) path = path[i:] i = 0 path_len = len(path) replan.modify_tree_and_path(rrt, path) rrt.p_current = path[0] recheck_goal = True flag_obstacle = replan.detectObstacle(rrt, path, new_obstacle) if flag_obstacle is True: print('Obstacle encountered') sigma_current, sigma_separate, obstructed_path_ids, blocked_tree_indices = replan.removeOccupiedNodes( rrt, path, i, new_obstacle, True) plot(rrt.nodes, robot_pose, obstacle_pose, plot_no, sigma_current, True, True, sigma_separate) plot_no += 1 replan.modify_tree_and_path(rrt, sigma_current, sigma_separate, blocked_tree_indices) plot(rrt.nodes, robot_pose, obstacle_pose, plot_no, sigma_current, True, True, sigma_separate) plot_no += 1 rrt.p_current = sigma_current[0] print('Reconnecting.....') flag_reconnect, new_path = replan.reconnect( rrt, sigma_current, sigma_separate) if flag_reconnect: path = new_path i = 0 path_len = len(path) plot(rrt.nodes, robot_pose, obstacle_pose, plot_no, path, True) plot_no += 1 else: print('Reconnect failed trying regrow') temp = -1 * np.ones((3000, 6)) rrt.nodes = np.vstack((rrt.nodes, temp)) rrt.nodes[:, 5] = np.arange(rrt.nodes.shape[0]) for i in range(1500): replan.regrow(rrt, new_obstacle, sigma_separate[0]) flag_reconnect, new_path = replan.reconnect( rrt, sigma_current, sigma_separate) if flag_reconnect is True: path = new_path i = 0 path_len = len(path) plot(rrt.nodes, robot_pose, obstacle_pose, plot_no, path, True) plot_no += 1 else: print('Regrow failed') else: plot(rrt.nodes, robot_pose, obstacle_pose, plot_no, path[i:], True) plot_no += 1 if recheck_goal is True: if (plot_no != 0): flag, min_cost_idx = replan.check_goal(rrt) path, cost = replan.find_path(rrt, min_cost_idx) i = 0 path_len = len(path) step = step + 1