def get_samples_from_new_map():
for _ in range(20):
try:
with ExpectTimeout(3):
world = World.fixed_block(lower_bounds=(-2, -2, 0),
upper_bounds=(3, 2, 2),
block_width=0.5,
block_height=1.5,
num_blocks=4,
robot_radii=0.25,
margin=0.2)
break
except TimeoutError:
pass
resolution = (.25, .25, .25)
margin = .2
occ_map = OccupancyMap(world, resolution, margin)
start = world.world['start'] # Start point, shape=(3,)
goal = world.world['goal'] # Goal point, shape=(3,)
my_path = graph_search(world, resolution, margin, start, goal, False)[1:-1]
start = my_path[0]
goal = my_path[-1]
args = get_args()
args.go = goal
args.st = start
args.world = world
args.occ_map = occ_map
action_List = np.zeros((my_path.shape))
discretized_path = np.zeros((my_path.shape))
for i in range(discretized_path.shape[0]):
discretized_path[i, :] = args.occ_map.metric_to_index(my_path[i, :])
for i in range(discretized_path.shape[0]):
try:
action_List[i, :] = discretized_path[i + 1] - discretized_path[i]
except:
action_List[i, :] = np.zeros(3)
discretized_path = discretized_path.astype(int)
action_List = action_List.astype(int)
for i in range(len(action_List)):
args.optimal_path_length += np.linalg.norm(action_List[i])
start_index = discretized_path[0]
goal_index = discretized_path[-1]
args.start = start_index
args.goal = goal_index
args.search_range = 20
warm_up_set, warm_up_labels = get_all_warm_up(args, discretized_path,
action_List)
return args, warm_up_set, warm_up_labels
from flightsim.crazyflie_params import quad_params
from flightsim.simulate import Quadrotor, simulate, ExitStatus
from flightsim.world import World
from flightsim.world import ExpectTimeout
import numpy as np
quadrotor = Quadrotor(quad_params)
robot_radius = 0.25
fig = plt.figure('A* Path, Waypoints, and Trajectory')
ax = Axes3Ds(fig)
for _ in range(20):
try:
with ExpectTimeout(3):
world = World.fixed_block(lower_bounds=(-2,-2,0),upper_bounds=(3,2,2),block_width=0.5,block_height=1.5,num_blocks=4,robot_radii=robot_radius,margin=0.2)
break
except TimeoutError:
pass
world.draw(ax)
start=world.world['start']
goal=world.world['goal']
ax.plot([start[0]], [start[1]], [start[2]], 'go', markersize=5, markeredgewidth=3, markerfacecolor='none')
ax.plot([goal[0]], [goal[1]], [goal[2]], 'ro', markersize=5, markeredgewidth=3, markerfacecolor='none')
plt.show()
