def load_elements_by_random(self):
map = Map((int(self.cfg['MAP']['grid_row_dimension']),
int(self.cfg['MAP']['grid_column_dimension'])))
count = 0
while count <= int(self.cfg['MAP']['random_init_blocks_num']):
x = random.randint(0,
int(self.cfg['MAP']['grid_row_dimension']) - 1)
y = random.randint(
0,
int(self.cfg['MAP']['grid_column_dimension']) - 1)
if (x, y) not in map.blocks:
count += 1
map.grid[x][y] = BLOCK_AREA
map.blocks.append((x, y))
count = 0
while count <= int(self.cfg['MAP']['random_init_robots_num']):
x = random.randint(0,
int(self.cfg['MAP']['grid_row_dimension']) - 1)
y = random.randint(
0,
int(self.cfg['MAP']['grid_column_dimension']) - 1)
if (x, y) not in map.blocks:
count += 1
map.grid[x][y] = ROBOT_AREA
map.robots.append(Robot(x, y)) # add robots
return map
def load_elements_from_file(self, robot_init_filename, block_init_filename):
map = Map((int(self.cfg['MAP']['grid_row_dimension']), int(self.cfg['MAP']['grid_column_dimension'])))
for x, y in map.get_blocks_init_place(block_init_filename):
if not map.is_location_in_environment(x, y, (
int(self.cfg['MAP']['grid_row_dimension']), int(self.cfg['MAP']['grid_column_dimension']))):
raise OutsideBoundryError('init block (x, y) not in map!')
map.grid[x][y] = BLOCK_AREA
map.blocks.append((x, y))
for x, y in map.get_robot_init_place(robot_init_filename):
if not map.is_location_in_environment(x, y, (
int(self.cfg['MAP']['grid_row_dimension']), int(self.cfg['MAP']['grid_column_dimension']))):
raise OutsideBoundryError('init robot (x, y) not in map!')
map.grid[x][y] = ROBOT_AREA
map.robots.append(Robot(x, y)) # add robots
return map
def load_elements_by_click(self, robots_sequences, blocks_sequences):
map = Map((int(self.cfg['MAP']['grid_row_dimension']), int(self.cfg['MAP']['grid_column_dimension'])))
for x, y in blocks_sequences:
if not map.is_location_in_environment(x, y, (
int(self.cfg['MAP']['grid_row_dimension']), int(self.cfg['MAP']['grid_column_dimension']))):
raise OutsideBoundryError('init block (x, y) not in map!')
map.grid[x][y] = BLOCK_AREA
map.blocks.append((x, y))
for x, y in robots_sequences:
if not map.is_location_in_environment(x, y, (
int(self.cfg['MAP']['grid_row_dimension']), int(self.cfg['MAP']['grid_column_dimension']))):
raise OutsideBoundryError('init robot (x, y) not in map!')
map.grid[x][y] = ROBOT_AREA
map.robots.append(Robot(x, y)) # add robots
return map
def main():
print("start!!")
# load map
src_path_map = "data/two_obs.dat"
map1 = Map(src_path_map)
# initialize robot (initializes lidar with map)
robbie = Robot(map1)
for i in range(100):
print("Time " + str(i))
plt.cla()
robbie.update()
map1.visualize_map()
robbie.visualize()
plt.pause(0.1)
print("done!!")
2) The second particle filter resamples if the approximate number of effective particles drops below a pre-defined
threshold (NEPR)
3) The third particle filter resamples in case the reciprocal of the maximum weight drops below a pre-defined
threshold (MWR)
"""
print("Compare three resampling schemes.")
# Define simulated world
world = World(world_size_x, world_size_y, landmark_positions)
# Initialize simulated robot
robot = Robot(x=world.x_max * 0.75,
y=world.y_max / 5.0,
theta=3.14 / 2.0,
std_forward=true_robot_motion_forward_std,
std_turn=true_robot_motion_turn_std,
std_meas_distance=true_robot_meas_noise_distance_std,
std_meas_angle=true_robot_meas_noise_angle_std)
# Set resampling algorithm used (same for all filters)
resampling_algorithm = ResamplingAlgorithms.MULTINOMIAL
# Number of particles in the particle filters
number_of_particles = 1000
# Resampling thresholds
number_of_effective_particles_threshold = number_of_particles / 4.0
reciprocal_max_weight_resampling_threshold = 1.0 / 0.005
##
##
# Simulated world
world = World(world_size_x, world_size_y, landmark_positions)
# Initialize visualization
show_particle_pose = False # only set to true for low #particles (very slow)
visualizer = Visualizer(show_particle_pose)
# Number of simulated time steps
n_time_steps = 15
# Simulated robot
robot = Robot(robot_initial_x_position, robot_initial_y_position,
robot_initial_heading, true_robot_motion_forward_std,
true_robot_motion_turn_std,
true_robot_meas_noise_distance_std,
true_robot_meas_noise_angle_std)
# Number of particles
number_of_particles = 500
# Set resampling algorithm used
resampling_algorithm = ResamplingAlgorithms.MULTINOMIAL
# Initialize the particle filter
# Initialize SIR particle filter
particle_filter_sir = ParticleFilterSIR(number_of_particles,
pf_state_limits, process_noise,
measurement_noise,
def main():
# Instantiate Map
src_path_map = "data/two_obs.dat"
map1 = Map(src_path_map)
# Instantiate dense lidar for evaluation
dense_lidar = LidarSimulator(map1, angles=np.arange(90) * 4)
# Instantiate Robot to be evaluated
safe_robbie = Robot(map1, use_safe=True)
unsafe_robbie = Robot(map1, use_safe=False)
# Instantiate list to store closest distance over time
safe_closest_list = []
unsafe_closest_list = []
for i in range(100):
plt.cla()
# Move robot
safe_robbie.update()
unsafe_robbie.update()
# Evaluation: Get distance to closest obstacle w/ dense lidar
safe_closest = distance_to_closest_obstacle(dense_lidar, safe_robbie)
safe_closest_list.append(safe_closest)
unsafe_closest = distance_to_closest_obstacle(dense_lidar,
unsafe_robbie)
unsafe_closest_list.append(unsafe_closest)
# TODO: write to text file
# # Visualize
# map1.visualize_map()
# safe_robbie.visualize()
# plt.pause(0.1)
# Visualize history
map1.visualize_map()
safe_robbie.visualize_robot()
unsafe_robbie.visualize_robot()
custom_legend = [
Line2D([0], [0], color=color_cycle[0], lw=4),
Line2D([0], [0], color=color_cycle[1], lw=4)
]
plt.legend(custom_legend, ["Safe Control", "Unsafe Control"])
# Plot Closest Distance over Time
plt.figure()
plt.plot(range(len(safe_closest_list)), safe_closest_list, label="Safe")
plt.plot(range(len(unsafe_closest_list)),
unsafe_closest_list,
label="Unsafe")
plt.legend()
plt.xlabel("Time")
plt.ylabel("Distance to Closest Obstacle (m)")
plt.show()