def main_heapq():
""" Works with finer grid, with inflated points for start,goal, and obstacles """
#assigning status name to color: start -> red (start) o -> black (obstacle), p -> green (path), g -> blue (goal)
p = 0
s = 1
g = 2
o = 3
#set up the real map parameters:
x_range = [-2, 5] #cln
y_range = [-6, 6] #row
cell_size = 0.1
start_goal_table = [[[2.45, -3.55], [0.95, -1.55]],
[[4.95, -0.05], [2.45, 0.25]],
[[-0.55, 1.45], [1.95, 3.95]]]
#loading landmarks
landmark_table = load_landmark_data()
#set up the data matrix parameters
row_num = int((y_range[1] - y_range[0]) / cell_size)
cln_num = int((x_range[1] - x_range[0]) / cell_size)
# fill in the map data for each path. A figure will show once the previous one is closed.
for path in start_goal_table:
# make an empty data set
data = np.ones((row_num, cln_num)) * np.nan
# [start_coord,goal_coord], getting matrix coord of start and goal
start_mat_coord = to_matrix_coord(path[0], x_range, y_range, cell_size)
goal_mat_coord = to_matrix_coord(path[1], x_range, y_range, cell_size)
inflate_pt_size = 7
# inflate landmarks
for landmark in landmark_table:
lm_mat_coord = to_matrix_coord(landmark, x_range, y_range,
cell_size)
inflated_pt_ls = inflate_point(lm_mat_coord, inflate_pt_size)
for pt in inflated_pt_ls:
if out_of_boudary(pt, row_num, cln_num) == False:
data[pt[0], pt[1]] = o
result = astar_heapq(data, start_mat_coord, goal_mat_coord)
for waypoint in result:
data[waypoint[0]][waypoint[1]] = p
#displaying the final result, after calculating the result
#inflate start and goal
inflated_start = inflate_point_end(start_mat_coord,
inflate_pt_size / 2)
inflated_goal = inflate_point_end(goal_mat_coord, inflate_pt_size / 2)
for pt in inflated_start:
if out_of_boudary(pt, row_num, cln_num) == False:
data[pt[0], pt[1]] = s
for pt in inflated_goal:
if out_of_boudary(pt, row_num, cln_num) == False:
data[pt[0], pt[1]] = g
display_grid(data, x_range, y_range, cell_size,
"World Map - A* Fine-Grid - heuristic 3")
def q11_coarse_main( show_arrows = False, add_noise = False):
#assigning status name to color: start -> red (start) o -> black (obstacle), p -> green (path), g -> blue (goal)
p = 0
s = 1
g = 2
o = 3
#set up the real map parameters:
x_range = [-2,5] #cln
y_range = [-6,6] #row
cell_size = 1
starts =[(0.5,-1.5),(4.5, 3.5),(-0.5, 5.5)]
goals = [(0.5, 1.5),(4.5,-1.5),(1.5,-3.5)]
#so start and goals are put in the form of [[start0,goal0],[start1,goal1]...].This is a 3d arra
start_goal_table = np.array([[starts[i],goals[i]] for i in range(len(starts))])
heading_init = -1*np.pi/2
#loading landmarks
landmark_table = load_landmark_data()
#set up the data matrix parameters
row_num = (y_range[1]-y_range[0])/cell_size
cln_num = (x_range[1]-x_range[0])/cell_size
# fill in the map data for each path. A figure will show once the previous one is closed.
for path in start_goal_table:
# make an empty data set
data = np.ones((row_num, cln_num)) * np.nan
# [start_coord,goal_coord], getting matrix coord of start and goal
start_mat_coord = to_matrix_coord(path[0],x_range,y_range,cell_size)
goal_mat_coord = to_matrix_coord(path[1],x_range,y_range,cell_size)
for landmark in landmark_table:
lm_mat_coord = to_matrix_coord( landmark, x_range,y_range,cell_size)
data[lm_mat_coord[0], lm_mat_coord[1]]=o
#issuing control here TODO
result, turtle_path = astar_online_control_coarse_map(data,start_mat_coord,goal_mat_coord, add_noise)
for waypoint in result:
data[waypoint[0]][waypoint[1]] = p
#displaying the final result, after calculating the result
data[ start_mat_coord[0], start_mat_coord[1]] = s
data[ goal_mat_coord[0], goal_mat_coord[1]] = g
#display_grid(data,x_range,y_range,cell_size,"World Map - Q11 Online Planning + Control")
display_grid_live(data, turtle_path, x_range,y_range,cell_size,"World Map - Q11 Online Planning + Control",show_arrows)
def main_offline():
#assigning status name to color: start -> red (start) o -> black (obstacle), p -> green (path), g -> blue (goal)
p = 0
s = 1
g = 2
o = 3
#set up the real map parameters:
x_range = [-2, 5] #cln
y_range = [-6, 6] #row
cell_size = 1
starts = [(0.5, -1.5), (4.5, 3.5), (-0.5, 5.5)]
goals = [(0.5, 1.5), (4.5, -1.5), (1.5, -3.5)]
#so start and goals are put in the form of [[start0,goal0],[start1,goal1]...].This is a 3d array.
start_goal_table = np.array([[starts[i], goals[i]]
for i in range(len(starts))])
#loading landmarks
landmark_table = load_landmark_data()
#set up the data matrix parameters
row_num = (y_range[1] - y_range[0]) / cell_size
cln_num = (x_range[1] - x_range[0]) / cell_size
# fill in the map data for each path. A figure will show once the previous one is closed.
for path in start_goal_table:
# make an empty data set
data = np.ones((row_num, cln_num)) * np.nan
# [start_coord,goal_coord], getting matrix coord of start and goal
start_mat_coord = to_matrix_coord(path[0], x_range, y_range, cell_size)
goal_mat_coord = to_matrix_coord(path[1], x_range, y_range, cell_size)
for landmark in landmark_table:
lm_mat_coord = to_matrix_coord(landmark, x_range, y_range,
cell_size)
data[lm_mat_coord[0], lm_mat_coord[1]] = o
result = astar(data, start_mat_coord, goal_mat_coord)
for waypoint in result:
data[waypoint[0]][waypoint[1]] = p
#displaying the final result, after calculating the result
data[start_mat_coord[0], start_mat_coord[1]] = s
data[goal_mat_coord[0], goal_mat_coord[1]] = g
display_grid(data, x_range, y_range, cell_size,
"World Map - A* Offline")
def main_q11_fine_grid(show_arrows = False, add_noise = False):
""" Works with finer grid, with inflated points for start,goal, and obstacles """
#configuration variables
#assigning status name to color: start -> red (start) o -> black (obstacle), p -> green (path), g -> blue (goal)
p = 0
s = 1
g = 2
o = 3
#set up the real map parameters:
x_range = [-2,5] #cln
y_range = [-6,6] #row
cell_size = 0.1
starts =[(0.5,-1.5),(4.5, 3.5),(-0.5, 5.5)]
goals = [(0.5, 1.5),(4.5,-1.5),(1.5,-3.5)]
#so start and goals are put in the form of [[start0,goal0],[start1,goal1]...].This is a 3d array.
start_goal_table = np.array([[starts[i],goals[i]] for i in range(len(starts))])
heading_init = -1*np.pi/2
#loading landmarks
landmark_table = load_landmark_data()
#set up the data matrix parameters
row_num = int((y_range[1]-y_range[0])/cell_size)
cln_num = int((x_range[1]-x_range[0])/cell_size)
# fill in the map data for each path. A figure will show once the previous one is closed.
for path in start_goal_table:
# make an empty data set
data = np.ones((row_num, cln_num)) * np.nan
# [start_coord,goal_coord], getting matrix coord of start and goal
start_mat_coord = to_matrix_coord(path[0],x_range,y_range,cell_size)
goal_mat_coord = to_matrix_coord(path[1],x_range,y_range,cell_size)
inflate_pt_size = 7
# inflate landmarks
for landmark in landmark_table:
lm_mat_coord = to_matrix_coord( landmark, x_range,y_range,cell_size)
inflated_pt_ls = inflate_point( lm_mat_coord, inflate_pt_size)
for pt in inflated_pt_ls:
if out_of_boudary(pt,row_num, cln_num) == False:
data[pt[0], pt[1]]=o
result,turtle_path = astar_heapq_online_control(data,start_mat_coord,goal_mat_coord, add_noise) #list
for waypoint in result[1:]:
data[waypoint[0]][waypoint[1]] = p
#displaying the final result, after calculating the result
#inflate start and goal
inflated_start = inflate_point_end( start_mat_coord, inflate_pt_size/2)
inflated_goal = inflate_point_end( goal_mat_coord,inflate_pt_size/2)
for pt in inflated_start:
if out_of_boudary(pt,row_num, cln_num) == False:
data[pt[0],pt[1]] = s
for pt in inflated_goal:
if out_of_boudary(pt,row_num, cln_num) == False:
data[pt[0],pt[1]] = g
display_grid_live(data, turtle_path, x_range,y_range,cell_size,"World Map - Controller + A* Fine-Grid" , show_arrows)
def main_q9(show_arrows=False, add_noise=False):
""" Works with finer grid, with inflated points for start,goal, and obstacles """
#configuration variables
#assigning status name to color: start -> red (start) o -> black (obstacle), p -> green (path), g -> blue (goal)
p = 0
s = 1
g = 2
o = 3
#set up the real map parameters:
x_range = [-2, 5] #cln
y_range = [-6, 6] #row
cell_size = 0.1
start_goal_table = [[[2.45, -3.55], [0.95, -1.55]],
[[4.95, -0.05], [2.45, 0.25]],
[[-0.55, 1.45], [1.95, 3.95]]]
heading_init = -1 * np.pi / 2
#loading landmarks
landmark_table = load_landmark_data()
#set up the data matrix parameters
row_num = int((y_range[1] - y_range[0]) / cell_size)
cln_num = int((x_range[1] - x_range[0]) / cell_size)
# fill in the map data for each path. A figure will show once the previous one is closed.
for path in start_goal_table:
# make an empty data set
data = np.ones((row_num, cln_num)) * np.nan
# [start_coord,goal_coord], getting matrix coord of start and goal
start_mat_coord = to_matrix_coord(path[0], x_range, y_range, cell_size)
goal_mat_coord = to_matrix_coord(path[1], x_range, y_range, cell_size)
inflate_pt_size = 7
# inflate landmarks
for landmark in landmark_table:
lm_mat_coord = to_matrix_coord(landmark, x_range, y_range,
cell_size)
inflated_pt_ls = inflate_point(lm_mat_coord, inflate_pt_size)
for pt in inflated_pt_ls:
if out_of_boudary(pt, row_num, cln_num) == False:
data[pt[0], pt[1]] = o
result = astar_heapq(data, start_mat_coord, goal_mat_coord) #list
turtle_path_start = path[0] #start in map coord for each way point
turtle_path_start.append(heading_init)
turtle_path_x = np.array([turtle_path_start[0]]) #1 x m np array
turtle_path_y = np.array([turtle_path_start[1]]) #1 x m np array
turtle_path_theta = np.array([turtle_path_start[2]]) #1 x m np array
for waypoint in result:
data[waypoint[0]][waypoint[1]] = p
#each waypoint in data is transformed into real map coordinate, then we get a set of turtle's path
control_end = to_map_coord(waypoint, x_range, y_range, cell_size)
control_end = np.append(control_end,
0) #adding dummy 0 to fit into control
x_vec, y_vec, theta_vec = control_interface(
turtle_path_start, control_end, cell_size, add_noise)
turtle_path_start = np.array([x_vec[-1], y_vec[-1], theta_vec[-1]])
turtle_path_x = np.append(turtle_path_x, x_vec) #1*m np array,
turtle_path_y = np.append(turtle_path_y, y_vec) #1*m np array
turtle_path_theta = np.append(turtle_path_theta,
theta_vec) #1*m np array
turtle_path = (turtle_path_x, turtle_path_y, turtle_path_theta)
#displaying the final result, after calculating the result
#inflate start and goal
inflated_start = inflate_point_end(start_mat_coord,
inflate_pt_size / 2)
inflated_goal = inflate_point_end(goal_mat_coord, inflate_pt_size / 2)
for pt in inflated_start:
if out_of_boudary(pt, row_num, cln_num) == False:
data[pt[0], pt[1]] = s
for pt in inflated_goal:
if out_of_boudary(pt, row_num, cln_num) == False:
data[pt[0], pt[1]] = g
display_grid_live(data, turtle_path, x_range, y_range, cell_size,
"World Map - Controller + A* Fine-Grid",
show_arrows)