def __init__(self): # build a maze and set the target sell self.maze_size = (6, 6) self.real_maze = Maze2(*self.maze_size) self.real_maze.generate_random_maze() # self.real_maze.load('utils/mini_flipped.maze') print self.real_maze # specify the initial state self.real_bot_state = np.array([ 0.5 * p.maze_cell_size, 0.5 * p.maze_cell_size, np.pi / 2, 0, 0, 0 ]) self.estimated_maze = Maze2(*self.maze_size) self.estimated_maze.v_walls[ 1:-1, :] = 1. # set all the confidences to an initial value self.estimated_maze.h_walls[:, 1:-1] = 1. self.estimated_maze.h_walls[ 0, 1] = 0. # the wall directly in front of the robot is clear # a noisey model for when the robot moves (should be the same as the estimator) # NOTE(izzy): this sigma should be estimated by the dyanmics model somehow??? # We might have to collect mocap data in order to get this self.u_sigma = np.array([.001, .001, np.radians(1), 1e-4, 1e-4, 1e-4]) # self.u_sigma = np.zeros(6) # noise to add to simulated sensor data self.encoder_bias = 1.03 self.lidar_bias = 1.02 self.imu_bias = np.radians(1) self.lidar_sigma = 0.025 # as a percent of the distance self.encoder_sigma = 0.05 # as a percent of the angular velocity self.imu_sigma = np.radians(2) # in radians self.estimator = Estimator( self.real_bot_state) # initialize the state estimator self.estimator.set_maze( self.estimated_maze, obs_func=lidar_observation_function_gaussian_multi ) # pass it the maze self.estimator.u_sigma = self.u_sigma # and the noise model self.plan = self.real_bot_state[:2] self.cmd = np.zeros(2) self.tremaux = Tremaux( self.estimated_maze ) # pass tremaux the maze just to set the width and height self.forward_increment = 0.5 self.steer_increment = 0.5 self.dt = 1 / 8.
def __init__(self): rospy.init_node('planner_node') self.plan_publisher = rospy.Publisher('/pacmouse/plan', Vector3, queue_size=1) self.pose = np.zeros(3) self.prev_plan = np.ones(2) * p.maze_cell_size / 2 # self.shortest_path_solving = False # self.maze = None self.shortest_path_solving = False self.maze = Maze2() self.maze.load('../utils/mini.maze') self.maze.build_adjacency_matrix( threshold=p.wall_transparency_threshold) self.maze.solve() self.tremaux = Tremaux(self.maze) self.goal_cell = np.zeros(2, dtype=int) rospy.Subscriber('/pacmouse/pose/mocap', Vector3, self.pose_callback) rospy.Subscriber('/pacmouse/pose/estimate', Vector3, self.pose_callback) rospy.Subscriber('/pacmouse/mode/set_plan_mode', String, self.mode_callback) rospy.Subscriber('/pacmouse/maze', Maze, self.maze_callback) rospy.Subscriber('/pacmouse/goal', Vector3, self.set_goal_for_testing) rospy.spin()
def test_update_walls(): maze = Maze2(3, 3) # maze.load('../utils/mini_flipped.maze') maze.generate_random_maze() # maze.v_walls[:,:] = 0.2 # maze.h_walls[:,:] = 0.2 maze.add_perimeter() # # plot the maze maze.build_segment_list() maze.plot(plt) border = p.maze_cell_size plt.xlim(-border, maze.width * p.maze_cell_size + border) plt.ylim(-border, maze.height * p.maze_cell_size + border) # generate poses pose = np.array([(np.random.randint(maze.width) + 0.5) * p.maze_cell_size, (np.random.randint(maze.height) + 0.5) * p.maze_cell_size, np.random.rand() * np.pi * 2]) # pose = np.array([p.maze_cell_size*1, p.maze_cell_size*1.5, -np.pi*.999]) lidars = estimate_lidar_returns_multi(pose[None, :], maze, debug_plot=False)[0] update_walls(pose, lidars, maze, debug_plot=plt) plt.show()
def test_estimate_lidar_returns_multi(): maze = Maze2(8, 8) maze.generate_random_maze() # maze.v_walls *= np.random.rand(*maze.v_walls.shape) # maze.h_walls *= np.random.rand(*maze.h_walls.shape) # # plot the maze maze.build_segment_list() maze.plot(plt) border = p.maze_cell_size plt.xlim(-border, maze.width * p.maze_cell_size + border) plt.ylim(-border, maze.height * p.maze_cell_size + border) # generate poses n = 3 poses = np.array([ np.random.rand(n) * maze.width * p.maze_cell_size, np.random.rand(n) * maze.height * p.maze_cell_size, np.random.rand(n) * np.pi * 2 ]).T lidars, confidences = estimate_lidar_returns_multi(poses, maze, return_confidences=True, debug_plot=plt) plt.show()
def __init__(self): rospy.init_node('navigation_node') self.initial_state = np.array( [p.maze_cell_size / 2, p.maze_cell_size / 2, 0., 0, 0, 0]) self.estimator = Estimator(self.initial_state, p.num_particles) self.estimator.set_maze(self.maze) self.maze = Maze2() self.locked_maze = Maze2() self.backup_counter = 0 self.lidars = np.zeros(6) self.encoders = np.zeros(2) self.imu = 0.0 self.prev_t = time.time() self.prev_plan = self.initial_state[:2] self.shortest_path_solving = False # publishers self.pose_pub = rospy.Publisher('/pacmouse/pose/estimate', Vector3, queue_size=1) self.plan_pub = rospy.Publisher('/pacmouse/plan', Vector3, queue_size=1) self.led_pub = rospy.Publisher('/pacmouse/mode/set_leds', LED, queue_size=1) # sensor callbacks for state estimation rospy.Subscriber('/pacmouse/lidars', Lidars, self.lidars_callback) rospy.Subscriber('/pacmouse/imu', Float64, self.imu_callback) rospy.Subscriber('/pacmouse/encoders/velocity', Drive, self.encoders_callback) # mode controller callbacks rospy.Subscriber('/pacmouse/mode/zero_pose', Empty, self.zero_pose_callback) rospy.Subscriber('/pacmouse/mode/load_maze', Int16, self.maze_backup_callback) rospy.Subscriber('/pacmouse/mode/set_plan_mode', String, self.mode_callback)
def load_backup(back): mazes = list_mazes() m = Maze2() if 0 < back <= len(mazes): i = mazes[-back] maze_file = num_to_maze(i) print 'maze_backup loading {}'.format(maze_file) m.load(maze_file) clear_mazes() print 'maze_backup reset' return m
def maze_callback(self, msg): w = msg.width h = msg.height m = Maze2(w, h) m.h_walls = np.array(msg.h_walls).reshape([w, h + 1]) m.h_walls = np.array(msg.v_walls).reshape([w + 1, h]) self.maze = m if self.shortest_path_solving: # use dijkstras to solve the pairwise distances when we are in shortest path mode self.maze.build_adjacency_matrix( threshold=p.wall_transparency_threshold) self.maze.solve()
def __init__(self): # build a maze and set the target sell self.maze_size = (8, 8) self.real_maze = Maze2(*self.maze_size) self.real_maze.generate_random_maze() print self.real_maze # temp_maze = Maze(*self.maze_size) # temp_maze.build_wall_matrices() # print temp_maze # self.real_maze.v_walls = 1 -temp_maze.v_walls # self.real_maze.h_walls = 1 -temp_maze.h_walls # specify the initial state self.real_bot_state = np.array([ 0.5 * p.maze_cell_size, 0.5 * p.maze_cell_size, np.pi / 2, 0, 0, 0 ]) # a nosie model for when the robot moves (should be the same as the estimator) # NOTE(izzy): this sigma should be estimated by the dyanmics model somehow??? # We might have to collect mocap data in order to get this self.u_sigma = np.array([.002, .002, np.radians(2), 1e-4, 1e-4, 1e-4]) # noise to add to simulated sensor data self.lidar_sigma = 0.005 self.encoder_sigma = 0.05 self.estimator = Estimator( self.real_bot_state) # initialize the state estimator self.estimator.set_maze(self.real_maze) # pass it the maze self.estimator.u_sigma = self.u_sigma # and the noise model self.estimated_maze = Maze2(*self.maze_size) # self.estimated_maze.v_walls[:,:] = 1. # self.estimated_maze.h_walls[:,:] = 1. self.estimated_maze.build_segment_list() self.cmd = np.zeros(2) self.forward_increment = 0.5 self.steer_increment = 0.5 self.dt = 0.1
def __init__(self): self.m = Maze2(4, 4) #self.m.generate_random_maze() self.m.set_wall_between(0, 1, 1) self.m.set_wall_between(4, 5, 1) self.m.set_wall_between(8, 12, 1) self.m.set_wall_between(9, 13, 1) self.m.set_wall_between(5, 6, 1) self.m.set_wall_between(6, 10, 1) self.m.set_wall_between(11, 15, 1) self.m.set_wall_between(3, 7, 1) self.m.build_segment_list() print(self.m) fig = plt.figure() self.ax1 = fig.add_subplot(1, 1, 1)
def __init__(self): self.origin = None self.target = None self.prev_time = time.time() self.maze = Maze2() self.maze.load("../utils/mini.maze") self.mode = 'coord' rospy.init_node('motor_encoder_tester') rospy.Subscriber('/pacmouse/pose/mocap', Vector3, self.pose_callback) self.cmd_pub = rospy.Publisher('/pacmouse/motor/cmd', Drive, queue_size=1) self.spin()
def __init__(self): rospy.init_node('estimation_node') self.initial_state = np.array([p.maze_cell_size/2, p.maze_cell_size/2, 0., 0, 0, 0]) self.estimator = Estimator(self.initial_state, p.num_particles) self.encoders = np.zeros(2) self.imu = 0.0 self.prev_t = time.time() self.maze = Maze2() self.maze.load('../utils/mini_flipped.maze') self.estimator.set_maze(self.maze, obs_func=lidar_observation_function_gaussian_multi) self.maze_pub = rospy.Publisher('/pacmouse/maze', Maze, queue_size=1) self.pose_pub = rospy.Publisher('/pacmouse/pose/estimate', Vector3, queue_size=1) rospy.Subscriber('/pacmouse/lidars', Lidars, self.lidars_callback) rospy.Subscriber('/pacmouse/imu', Float64, self.imu_callback) rospy.Subscriber('/pacmouse/encoders/velocity', Drive, self.encoders_callback) rospy.Subscriber('/pacmouse/mode/zero_pose', Empty, self.zero_pose_callback) rospy.spin()
for f in os.listdir(p.maze_backup_path) if f.endswith('.maze') ] def list_mazes(): maze_files = os.listdir(p.maze_backup_path) numbers = [] for maze_file in maze_files: try: maze_num = int(maze_file.split('.')[0]) numbers.append(maze_num) except: print 'maze_backup failed to parse {}'.format(maze_file) return sorted(numbers) def save_backup(maze, i): maze.save(num_to_maze(i)) def num_to_maze(i): return p.maze_backup_path + '/' + str(i) + '.maze' if __name__ == '__main__': for i in range(10): m = Maze2() save_backup(m, i) print load_backup(2)
# helper function for simulated exploration of the maze. the robot is able # to observe the walls around it. def observe(cell, real_maze, estimated_maze): i_x = cell % real_maze.width i_y = np.floor(cell/real_maze.width).astype(int) estimated_maze.v_walls[i_x, i_y] = real_maze.v_walls[i_x, i_y] estimated_maze.v_walls[i_x+1, i_y] = real_maze.v_walls[i_x+1, i_y] estimated_maze.h_walls[i_x, i_y] = real_maze.h_walls[i_x, i_y] estimated_maze.h_walls[i_x, i_y+1] = real_maze.h_walls[i_x, i_y+1] if __name__ == '__main__': # the ground truth maze which we're exploring w, h = 16, 16 real_maze = Maze2(w,h) real_maze.generate_random_maze() print real_maze # the maze that we build as we explore estimated_maze = Maze2(w,h) print estimated_maze # position of the vehicle current_cell = 0 target_cell = int(w * h/2) tremaux = Tremaux(real_maze) counter = 0 while current_cell != target_cell: