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