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.0, 0.5]),
]
params = Params()
flight_area_vertices = 2 * np.array([[-0.6, 0.8], [-0.9, -0.9],
[0.8, -0.8], [0.5, 0.9]])
gridmap = GridMap(flight_area_vertices)
gridmap.add_obstacles_to_grid_map(obstacles)
# x, y, yaw
pose = [
np.mean(flight_area_vertices[:, 0]),
np.mean(flight_area_vertices[:, 1]), -np.pi / 3
]
traj = pose[:2]
plt.figure(figsize=(10, 10))
gridmap.draw_map(obstacles)
plt.plot(pose[0], pose[1], 'ro', markersize=20, label='Initial position')
# while True:
for _ in range(params.numiters):
dv = 0.1 * params.vel
pose[0] += dv * np.cos(pose[2])
pose[1] += dv * np.sin(pose[2])
pose_grid = gridmap.meters2grid(pose[:2])
boundary = obstacle_check([pose_grid[0], pose_grid[1], pose[2]],
gridmap)
# print(boundary)
if boundary['right'] or boundary['front']:
pose = back_shift(pose, 0.03)
pose[2] -= np.pi / 2 * np.random.uniform(0.2, 0.6)
elif boundary['left']:
pose = back_shift(pose, 0.03)
pose[2] += np.pi / 2 * np.random.uniform(0.2, 0.6)
traj = np.vstack([traj, pose[:2]])
if params.animate:
plt.cla()
gridmap.draw_map(obstacles)
visualize(traj, pose, gridmap)
plt.pause(0.1)
visualize(traj, pose, gridmap)
plt.show()
# 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')
print('Mission is complete!')
drone.disconnect()
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()
def main():
rospy.init_node('random_walk')
params = Params()
flight_area_vertices = [[-0.6, 0.8], [-0.9, -0.9], [0.8, -0.8], [0.5, 0.9]]
# flight_area_vertices=[[-0.5, 0.5], [-0.5, -0.5], [0.5, -0.5], [0.5, 0.5]]
gridmap = GridMap(flight_area_vertices)
drone = DroneMultiranger(params.uri)
time.sleep(3)
drone.pose_home = drone.position
print('Home positions:', drone.pose_home)
if params.toFly:
prepare(drone)
raw_input('Press Enter to fly...')
takeoff(drone, params.flight_height)
# x, y, yaw
pose = [drone.pose_home[0], drone.pose_home[1], 0.0]
# pose = [grdimap.map_center[0], grdimap.map_center[1], 0.0]
traj = pose[:2]
plt.figure(figsize=(8, 8))
# while True:
for _ in range(params.numiters):
dv = 0.1 * params.vel
pose[0] += dv * np.cos(pose[2])
pose[1] += dv * np.sin(pose[2])
pose_grid = gridmap.meters2grid(pose[:2])
border = borders_check([pose_grid[0], pose_grid[1], pose[2]], gridmap)
# print(border)
if border['right'] or border['front']:
print('Border in FRONT or RIGHT')
pose = back_shift(pose, 0.03)
pose[2] -= np.pi / 2 * np.random.uniform(0.2, 0.6)
elif border['left']:
print('Border on the LEFT')
pose = back_shift(pose, 0.03)
pose[2] += np.pi / 2 * np.random.uniform(0.2, 0.6)
if params.toFly:
if is_close(
drone.measurement['front']
) and drone.measurement['left'] > drone.measurement['right']:
print('FRONT RIGHT')
pose = back_shift(pose, 0.05)
pose[2] += np.pi / 2 * np.random.uniform(0.1, 0.8)
if is_close(
drone.measurement['front']
) and drone.measurement['left'] < drone.measurement['right']:
print('FRONT LEFT')
pose = back_shift(pose, 0.05)
pose[2] += np.pi / 2 * np.random.uniform(0.1, 0.8)
if is_close(drone.measurement['left']):
print('LEFT')
pose = right_shift(pose, 0.05)
if is_close(drone.measurement['right']):
print('RIGHT')
pose = left_shift(pose, 0.05)
traj = np.vstack([traj, pose[:2]])
drone.sp = [
pose[0], pose[1], params.flight_height,
np.degrees(pose[2]) % 360
]
if params.check_battery:
try:
if drone.battery_state == 'needs_charging':
print('Going home to CHARGE the battery')
print('Battery status: %.2f [V]' % drone.V_bat)
break
except:
pass
if params.toFly:
fly(drone)
time.sleep(0.1)
else:
plt.cla()
gridmap.draw_map()
visualize(traj, pose, gridmap)
plt.pause(0.1)
gridmap.draw_map()
visualize(traj, pose, gridmap)
if params.toFly:
goTo(drone,
[drone.pose_home[0], drone.pose_home[1], params.flight_height, 0])
hover(drone, 1.0)
land(drone)
plt.show()
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.0, 0.5]),
]
params = Params()
flight_area_vertices = 2 * np.array([[-0.6, 0.8], [-0.9, -0.9],
[0.8, -0.8], [0.5, 0.9]])
gridmap = GridMap(flight_area_vertices)
gridmap.add_obstacles_to_grid_map(obstacles)
# goal = [x, y], m
goal = np.array([0.5, 0.9])
# initial state = [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
state = np.array([-1.0, -1.0, np.pi / 2, 0.0, 0.0])
traj = state[:2]
plt.figure(figsize=(10, 10))
gridmap.draw_map(obstacles)
# while True:
for _ in range(params.numiters):
state = motion(state, goal, params)
pose_grid = gridmap.meters2grid(state[:2])
boundary = obstacle_check([pose_grid[0], pose_grid[1], state[2]],
gridmap.gmap, params)
# print(boundary)
if boundary['right'] or boundary['front']:
# state = back_shift(state, 0.03)
state = slow_down(state, params)
state = turn_left(state, np.radians(20))
elif boundary['left']:
# state = back_shift(state, 0.03)
state = slow_down(state, params)
state = turn_right(state, np.radians(20))
goal_dist = np.linalg.norm(goal - state[:2])
# print('Distance to goal %.2f [m]:' %goal_dist)
if goal_dist < params.goal_tol: # goal is reached
print('Goal is reached.')
goal = np.array(
[np.random.uniform(-1, 1),
np.random.uniform(-1, 1)])
traj = np.vstack([traj, state[:2]])
if params.animate:
plt.cla()
gridmap.draw_map(obstacles)
plt.plot(goal[0],
goal[1],
'ro',
markersize=20,
label='Goal position')
visualize(traj, state, params)
plt.pause(0.1)
plt.plot(goal[0], goal[1], 'ro', markersize=20, label='Goal position')
visualize(traj, state, params)
plt.show()