def test_planning1(self):
ox = [0.0, 20.0, 50.0, 100.0, 130.0, 40.0, 0.0]
oy = [0.0, -20.0, 0.0, 30.0, 60.0, 80.0, 0.0]
reso = 5.0
px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT,
sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN,
)
self.assertTrue(len(px) >= 5)
px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.LEFT,
sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN,
)
self.assertTrue(len(px) >= 5)
px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT,
sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.UP,
)
self.assertTrue(len(px) >= 5)
px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT,
sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN,
)
self.assertTrue(len(px) >= 5)
def test_planning3(self):
ox = [0.0, 20.0, 50.0, 200.0, 130.0, 40.0, 0.0]
oy = [0.0, -80.0, 0.0, 30.0, 60.0, 80.0, 0.0]
resolution = 5.1
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=self.RIGHT,
sweeping_direction=self.DOWN,
)
self.assertGreater(len(px), 5)
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=self.LEFT,
sweeping_direction=self.DOWN,
)
self.assertGreater(len(px), 5)
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=self.RIGHT,
sweeping_direction=self.UP,
)
self.assertGreater(len(px), 5)
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=self.RIGHT,
sweeping_direction=self.DOWN,
)
self.assertGreater(len(px), 5)
def planning_animation(ox, oy, reso, params): # pragma: no cover
px, py = planning(ox, oy, reso)
P = np.hstack([np.array(px)[:, np.newaxis], np.array(py)[:, np.newaxis]])
traj_global = waypts2setpts(P)
# animation
if params.do_animation:
for ipx, ipy in zip(traj_global[:, 0], traj_global[:, 1]):
plt.cla()
plt.plot(ox, oy, "-xb")
plt.plot(px, py, "-r")
plt.plot(ipx, ipy, "or")
plt.axis("equal")
plt.grid(True)
plt.pause(0.1)
plt.cla()
plt.plot(ox, oy, "-xb")
plt.plot(px, py, "-r")
plt.axis("equal")
plt.grid(True)
plt.pause(0.1)
return traj_global
]
veh_coords = [[
1, 1, 0, -2
]] # array containing all vehicles in [x_0,y_0,x_fin,y_fin] format
initial_pose = []
initial_pose.append(veh_coords[0][0])
initial_pose.append(veh_coords[0][1])
# Define Search Region
flight_area_vertices = define_flight_area(initial_pose)
gridmap = GridMap(flight_area_vertices, initial_pose)
# gridmap.add_obstacles_to_grid_map(obstacles)
ox = flight_area_vertices[:, 0].tolist() + [flight_area_vertices[0, 0]]
oy = flight_area_vertices[:, 1].tolist() + [flight_area_vertices[0, 1]]
reso = 2.0
goal_x, goal_y, gmap, covmap = planning(ox, oy,
reso) #waypoint geneneration
#test for grid map
# gridmap.draw_map(gridobstacles)
# plt.plot(goal_x,goal_y,'o', color='b')
wp_coords = [[]]
print("length of waypoints:", len(goal_x))
plt.figure()
for i in range(len(goal_x)):
# plt.plot(goal_x[i],goal_y[i],'o', color='b')
wp_coords[0].append([goal_x[i], goal_y[i]])
# plt.show()
print("goal_x", goal_x)
print("length(goal_x)", len(goal_x))
print("goal_y", goal_y)
def main():
params = Params()
# initial state = [x(m), y(m), z(m), yaw(rad), v(m/s), omega(rad/s)]
state = np.array([0, 0.2, params.max_height, np.pi / 2, 0.0, 0.0])
traj = state[:3]
plt.figure(figsize=(10, 10))
flight_area_vertices = define_flight_area(state[:2])
# flight_area_vertices = np.array([[-1, -1], [-0.3, -1], [-0.3, -0.4], [0.3, -0.4], [0.3, -1], [1,-1], [1,1], [-1,1]])
gridmap = GridMap(flight_area_vertices, state[:2])
ox = flight_area_vertices[:, 0].tolist() + [flight_area_vertices[0, 0]]
oy = flight_area_vertices[:, 1].tolist() + [flight_area_vertices[0, 1]]
reso = params.sweep_resolution
goal_x2D, goal_y2D = planning(ox, oy, reso)
waypoints = get_3D_waypoints(goal_x2D, goal_y2D, params.min_height,
params.max_height,
params.sweep_resolution / 2.)
goal_x = waypoints[:, 0]
goal_y = waypoints[:, 1]
goal_z = waypoints[:, 2]
# goal = [x, y], m
goali = 0
goal = [goal_x[goali], goal_y[goali], goal_z[goali]]
t_prev_goal = time.time()
fig = plt.figure(figsize=(10, 10))
ax = plt.axes(projection='3d')
ax.set_xlabel('X, [m]')
ax.set_ylabel('Y, [m]')
ax.set_zlabel('Z, [m]')
ax.set_xlim([-2.5, 2.5])
ax.set_ylim([-2.5, 2.5])
ax.set_zlim([0.0, 3.0])
# while True:
for _ in tqdm(range(params.numiters)):
state = motion(state, goal, params)
state = collision_avoidance(state, gridmap, params)
goal_dist = np.linalg.norm(goal - state[:3])
# print('Distance to goal %.2f [m]:' %goal_dist)
t_current = time.time()
if goal_dist < params.goal_tol or (
t_current -
t_prev_goal) > params.time_to_switch_goal: # goal is reached
# print('Switching to the next goal.')
# print('Time from the previous reached goal:', t_current - t_prev_goal)
if goali < len(goal_x) - 1:
goali += 1
else:
break
t_prev_goal = time.time()
goal = [goal_x[goali], goal_y[goali], goal_z[goali]]
traj = np.vstack([traj, state[:3]])
if params.animate:
plt.cla()
ax.plot(goal_x, goal_y, goal_z, ':')
ax.scatter(goal[0],
goal[1],
goal[2],
label='Goal position',
zorder=20)
ax.plot(traj[:, 0], traj[:, 1], traj[:, 2], linewidth=3, color='g')
plot_robot(ax, state, params)
plt.legend()
plt.pause(0.1)
print('Mission is complete!')
plt.cla()
ax.plot(goal_x, goal_y, goal_z, ':')
ax.scatter(goal[0], goal[1], goal[2], label='Goal position', zorder=20)
ax.plot(traj[:, 0], traj[:, 1], traj[:, 2], linewidth=3, color='g')
plot_robot(ax, state, params)
plt.legend()
plt.pause(0.1)
plt.draw()
input('Hit Enter to close all figures')
plt.close('all')
print('Home positions:', drone.pose_home)
flight_area_vertices = define_flight_area(drone.pose_home, params)
# SCALE = 1.5; flight_area_vertices = SCALE * np.array([[-0.6, 0.8], [-0.9, -0.9], [0.8, -0.8], [0.5, 0.9]])
gridmap = GridMap(flight_area_vertices, drone.pose_home[:2])
# Adding virtual obstacles for debugging
obstacles = [
# np.array([[-0.2, 0.35], [-0.2, -0.35], [0.2, -0.35], [0.2, 0.35]])
]
gridmap.add_virtual_rectangular_obstacles(obstacles)
ox = flight_area_vertices[:, 0].tolist() + [flight_area_vertices[0, 0]]
oy = flight_area_vertices[:, 1].tolist() + [flight_area_vertices[0, 1]]
goal_x, goal_y = planning(ox, oy, params.sweep_resolution)
if params.toFly:
prepare(drone)
raw_input('Press Enter to fly...')
exploration_conveyer(drone, goal_x, goal_y, params)
gridmap.draw_map(obstacles)
plt.plot(goal_x, goal_y)
visualize(drone.traj, drone.state, params)
plt.draw()
plt.pause(0.1)
raw_input('Hit Enter to close all figures.')
plt.close('all')
def main():
obstacles = [
# np.array([[0.7, -0.9], [1.3, -0.9], [1.3, -0.8], [0.7, -0.8]]) + np.array([-1.0, 0.5]),
# np.array([[0.7, -0.9], [1.3, -0.9], [1.3, -0.8], [0.7, -0.8]]) + np.array([-1.0, 1.0]),
# np.array([[0.7, -0.9], [0.8, -0.9], [0.8, -0.3], [0.7, -0.3]]) + np.array([-1.5, 1.0]),
np.array([[-0.3, -0.4], [0.3, -0.4], [0.3, 0.1], [-0.3, 0.1]]) * 0.5
]
# initial state = [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
state = np.array([0, 0.2, np.pi / 2, 0.0, 0.0])
traj = state[:2]
params = Params()
# plt.figure(figsize=(10,10))
flight_area_vertices = define_flight_area(state[:2])
posset = []
# flight_area_vertices = np.array([[-1, -1], [-0.3, -1], [-0.3, -0.4], [0.3, -0.4], [0.3, -1], [1,-1], [1,1], [-1,1]])
gridmap = GridMap(flight_area_vertices, state[:2])
gridmap.add_obstacles_to_grid_map(obstacles)
#obstacle x, y coordinates
ox = flight_area_vertices[:, 0].tolist() + [flight_area_vertices[0, 0]]
oy = flight_area_vertices[:, 1].tolist() + [flight_area_vertices[0, 1]]
reso = params.sweep_resolution
goal_x, goal_y, gmap, covmap = planning(ox, oy, reso)
# covmap.plot_grid_map(axes[0])
# goal = [x, y], m
goali = 0
goal = [goal_x[goali], goal_y[goali]]
t_prev_goal = time.time()
gridmap.draw_map(obstacles)
iter = 0
# while True:
for _ in range(params.numiters):
state = motion(state, goal, params)
state = collision_avoidance(state, gridmap, params)
posset.append([state[0], state[1]])
update_coveragemap(state, covmap)
goal_dist = np.linalg.norm(goal - state[:2])
# print('Distance to goal %.2f [m]:' %goal_dist)
t_current = time.time()
# if goal_dist < params.goal_tol or (t_current - t_prev_goal) > params.time_to_switch_goal: # goal is reached
if goal_dist < params.goal_tol: # goal is reached
print('Switching to the next goal.')
print('Time from the previous reached goal:',
t_current - t_prev_goal)
if goali < len(goal_x) - 1:
goali += 1
else:
break
t_prev_goal = time.time()
goal = [goal_x[goali], goal_y[goali]]
traj = np.vstack([traj, state[:2]])
if params.animate:
axes[1].cla()
# plt.cla()
gridmap.draw_map(obstacles, axes[1]) #mk
axes[1].plot(goal_x, goal_y)
axes[1].plot(goal[0],
goal[1],
'ro',
markersize=12,
label='Goal position',
zorder=20)
visualize(traj, state, params)
visualize_coverage(posset)
# plt.plot(goal_x, goal_y)
# plt.plot(goal[0], goal[1], 'ro', markersize=12, label='Goal position', zorder=20)
# visualize(traj, state, params)
# visualize_coverage(posset)
covmap.plot_grid_map(axes[0])
plt.pause(0.01)
iter = iter + 1
if iter == 1:
plt.savefig('planned_coverage_path.png', dpi=300)
# covmap2.plot_grid_map(axes[0])
print('Mission is complete!')
plt.plot(goal_x, goal_y)
visualize(traj, state, params)
plt.show()