def __init__(self):
self.gps_plot = RobotPlot("gps", color="red")
self.compass_plot = RobotPlot("compass", color="purple")
self.sticky_compass_plot = RobotPlot("sticky compass", color="gray")
self.sticky_compass_counter = 0
self.sticky_compass_skip = 100
self.accuracy_check_plot = RobotPlotCollection(
"Animation", self.sticky_compass_plot, self.gps_plot,
self.compass_plot)
self.plotter = LivePlotter(
2,
self.accuracy_check_plot,
matplotlib_events=dict(key_press_event=self.key_press))
self.gps = AdafruitGPS()
self.imu = BNO055()
# whoiam IDs were different in the old logs, changing for convenience, please don't do this otherwise!!
self.gps.whoiam = "gps"
self.imu.whoiam = "imu"
# file_name, directory = file_sets["rolls day 3"][0]
super(AnimationExample, self).__init__(self.imu, self.gps)
self.compass_angle = 0.0
self.start_angle = 0.0
self.link(self.gps, self.receive_gps)
self.link(self.imu, self.receive_imu)
def __init__(self):
self.tut = GPS()
self.time_plot = RobotPlot("gps time")
self.minute_plot = RobotPlot("gps minutes")
self.plotter = LivePlotter(2, self.time_plot, self.minute_plot)
super(TutRunner, self).__init__(
self.tut,
debug_prints=True,
log_data=False,
)
def __init__(self, enable_plotting=True):
self.turret = LidarTurret()
self.slam = SLAM(self.turret)
super(LidarTest, self).__init__(self.turret)
self.plotter = LivePlotter(
1, self.turret.point_cloud_plot, enabled=enable_plotting
)
self.slam_plots = SlamPlots(self.slam.map_size_pixels, self.slam.map_size_meters, skip_count=0)
self.lidar_t0 = 0.0
self.link_object(self.turret, self.received_lidar)
def __init__(self, animate, enable_plotting, enable_kalman, course_map,
inner_map, outer_map, key_press_fn, map_set_name,
label_checkpoints):
# GPS map based plots
self.gps_plot = RobotPlot("gps", color="red", enabled=True)
self.corrected_gps_plot = RobotPlot("corrected gps",
color="fuchsia",
enabled=True)
self.map_plot = RobotPlot("map",
color="purple",
marker='.',
put_in_legend=False)
self.inner_map_plot = RobotPlot("inner map",
enabled=True,
put_in_legend=False)
self.outer_map_plot = RobotPlot("outer map",
enabled=True,
put_in_legend=False)
# angle based plots
self.compass_plot = RobotPlot("quasar filtered heading",
color="purple",
put_in_legend=False)
self.compass_plot_imu_only = RobotPlot("imu filtered heading",
color="magenta",
put_in_legend=False)
self.sticky_compass_plot = RobotPlot("sticky compass",
color="gray",
enabled=False,
put_in_legend=False)
self.steering_plot = RobotPlot("steering angle",
color="gray",
enabled=False,
put_in_legend=False)
# indicator dots and lines
self.checkpoint_plot = RobotPlot("checkpoint",
color="green",
marker='.',
linestyle='',
markersize=8,
put_in_legend=False)
self.goal_plot = RobotPlot("checkpoint goal",
color="cyan",
put_in_legend=False)
self.recorded_goal_plot = RobotPlot("recorded checkpoint goal",
"blue",
put_in_legend=False)
self.current_pos_dot = RobotPlot("current pos dot",
"blue",
marker='.',
markersize=8,
put_in_legend=False)
# kalman plots
self.enable_kalman = enable_kalman
self.kalman_recomputed_plot = RobotPlot("kalman recomputed",
color="lawngreen",
enabled=self.enable_kalman)
self.kalman_recorded_plot = RobotPlot("kalman recorded",
color="gray",
enabled=self.enable_kalman)
self.steering_angle_plot = RobotPlot("steering angle",
put_in_legend=False)
self.sticky_compass_counter = 0
self.sticky_compass_skip = 100
self.label_checkpoints = label_checkpoints
# plot collection
self.accuracy_check_plot = RobotPlotCollection(
"RoboQuasar plot",
self.sticky_compass_plot,
self.gps_plot,
self.corrected_gps_plot,
self.checkpoint_plot,
self.map_plot,
self.inner_map_plot,
self.outer_map_plot,
self.steering_plot,
self.compass_plot_imu_only,
self.compass_plot,
self.goal_plot,
self.current_pos_dot,
self.kalman_recomputed_plot,
self.kalman_recorded_plot,
self.steering_angle_plot,
window_resizing=False)
if animate:
self.plotter = LivePlotter(
2,
self.accuracy_check_plot, # self.pipeline_plots,
matplotlib_events=dict(key_press_event=key_press_fn),
enable_legend=True,
enabled=enable_plotting,
skip_count=3)
else:
self.plotter = StaticPlotter(
1,
self.accuracy_check_plot,
matplotlib_events=dict(key_press_event=key_press_fn),
enabled=enable_plotting)
# plot maps
self.update_maps(course_map, inner_map, outer_map)
self.plot_image(map_set_name)
self.position_state = ""
self.prev_pos_state = self.position_state
def __init__(self, set_num=-1):
self.gps = GPS()
self.imu = IMU()
self.turret = LidarTurret()
self.steering = Steering()
self.brakes = Brakes()
self.underglow = Underglow()
joystick = None
if only_lidar:
self.gps.enabled = False
self.imu.enabled = False
self.steering.enabled = False
self.brakes.enabled = False
self.underglow.enabled = False
elif not simulated:
joystick = WiiUJoystick()
if args.includelidar:
self.turret.enabled = False
# self.imu_plot = RobotPlot("Magnetometer data (x, z vs. time)", flat_plot=True, skip_count=20,
# plot_enabled=False)
self.kalman_plots = KalmanPlots(not only_lidar, use_filter, animate)
if animate:
if args.includelidar:
self.turret.point_cloud_plot.enabled = False
self.live_plot = LivePlotter(2,
self.kalman_plots.filter_comparison,
self.turret.point_cloud_plot,
skip_count=10)
if self.turret.point_cloud_plot.enabled:
self.live_plot.draw_dot(self.turret.point_cloud_plot,
0,
0,
color='orange',
markersize=5)
elif plots_enabled:
self.static_plot = StaticPlotter(
1, self.kalman_plots.filter_comparison)
self.filter = None
# m = np.array(MapFile("buggy course checkpoints").map)
# self.map_plot.update(m[:, 0], m[:, 1])
file_sets = (
("16;49", "2017_Feb_17"), # default, latest
# data day 4
# ("15", "data_days/2017_Feb_14"), # filter explodes, LIDAR interfered by the sun
# ("16;20", "data_days/2017_Feb_14"), # shorten run, LIDAR collapsed
# ("16;57", "data_days/2017_Feb_14"), # interfered LIDAR
# ("17;10", "data_days/2017_Feb_14"), # all data is fine, interfered LIDAR
# ("17;33", "data_days/2017_Feb_14"), # data is fine, normal run
# data day 3
# ("16;38", "data_days/2017_Feb_08"),
# ("17", "data_days/2017_Feb_08"),
# ("18", "data_days/2017_Feb_08"),
# trackfield no gyro values
# ("15;46", "old_data/2016_Dec_02"),
# ("15;54", "old_data/2016_Dec_02"),
# ("16;10", "old_data/2016_Dec_02"),
# ("16;10", "old_data/2016_Dec_02")
# rolls day 1
# ("07;22", "old_data/2016_Nov_06"), # bad gyro values
# rolls day 2
# ("07;36;03 m", "old_data/2016_Nov_12"),
# ("09;12", "old_data/2016_Nov_12"), # invalid values
# ("07;04;57", "old_data/2016_Nov_13"), # bad gyro values
# rolls day 3
# ("modified 07;04", "old_data/2016_Nov_13"),
# ("modified 07;23", "old_data/2016_Nov_13"), # wonky value for mag.
# rolling on the cut
# ("16;29", "old_data/2016_Dec_09"),
# ("16;49", "old_data/2016_Dec_09"),
# ("16;5", "old_data/2016_Dec_09"),
# ("17;", "old_data/2016_Dec_09"), # nothing wrong, really short
# bad data
# ("16;07", "old_data/2016_Dec_09/bad_data"), # nothing wrong, really short
# ("16;09", "old_data/2016_Dec_09/bad_data"), # nothing wrong, really short
# ("18;00", "old_data/2016_Dec_09/bad_data"), # gps spazzed out
# ("18;02", "old_data/2016_Dec_09/bad_data"), # gps spazzed out
# ("18;09", "old_data/2016_Dec_09/bad_data"), # gps spazzed out
)
self.checkpoint_num = 0
self.lat1, self.long1, self.alt1 = 0, 0, 0
self.lat2, self.long2, self.alt2 = 0, 0, 0
self.num_recv_from_fix = None
self.received_fix = False
self.imu_t0 = 0
self.gps_t0 = 0
self.lidar_t0 = 0
if simulated:
super(RoboQuasar, self).__init__(
file_sets[set_num][0],
file_sets[set_num][1],
self.gps,
self.imu,
self.turret,
start_index=10000,
# end_index=2000,
)
extension_index = self.parser.full_path.rfind(".")
image_path = self.parser.full_path[:extension_index]
else:
super(RoboQuasar, self).__init__(self.imu,
self.gps,
self.steering,
self.brakes,
self.underglow,
self.turret,
joystick=joystick,
debug_prints=args.verbose,
log_data=args.log)
extension_index = self.logger.full_path.rfind(".")
image_path = self.logger.full_path[:extension_index]
self.slam = SLAM(self.turret, image_path + " post map")
self.slam_plots = SlamPlots(self.slam.map_size_pixels,
self.slam.map_size_meters,
args.computeslam)