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