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():
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()
print("pedestrian initial state: ", pedestrian_state)
print("pedestrian initial goal : ", pedestrian_goal)
flight_area_vertices = [[-12.5, 12.5], [12.5, 12.5], [12.5, -12.5],
[-12.5, -12.5]]
gridmap = GridMap(flight_area_vertices, state[:2])
# fill the obstacles_array
for i in range(len(obstacles)):
tmp = np.array([
obstacles[i].vertices[0], obstacles[i].vertices[1],
obstacles[i].vertices[2], obstacles[i].vertices[3]
])
obstacles_array.append(tmp)
gridmap.add_obstacles_to_grid_map(obstacles_array)
for _ in range(params.numiters):
observation.get_obs(state, target_state, pedestrian_state, params,
target_goal, pedestrian_goal, params_localmap,
human_params_localmap, gridmap)
state, inputs = motion_dwa(state, inputs, goal, obpoints, walls,
params)
target_bool = True
target_state = human_motion(target_state, target_goal, human_params,
goal_tol,
target_bool) # human target motion model
target_state = collision_avoidance(target_state, gridmap, human_params)
target_bool = False
pedestrian_state = human_motion(pedestrian_state, pedestrian_goal,
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()
