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