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: