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