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()
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()
