class LidarTest(Robot):
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 received_lidar(self, timestamp, packet, packet_type):
if self.turret.did_cloud_update():
if self.turret.is_cloud_ready():
self.slam.update(timestamp, self.turret.distances, [0, 0, self.dt() - self.lidar_t0])
self.slam_plots.update(self.slam.mapbytes, self.slam.get_pos())
self.lidar_t0 = self.dt()
return self.plotter.plot()
def close(self, reason):
if reason == "done":
self.plotter.freeze_plot()
self.plotter.close()
self.slam.make_image(self.logger.full_path + " post map")
class TutRunner(RobotInterface):
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 loop(self):
pass
def packet_received(self, timestamp, whoiam, packet):
if self.did_receive(self.tut):
if self.queue_len() < 25:
self.time_plot.append(timestamp, self.tut.second)
self.minute_plot.append(timestamp, self.tut.minute)
if not self.plotter.plot():
return False
def close(self):
self.plotter.close()
def start(self):
self.plotter.start_time(self.start_time)
class RoboQuasar(Interface):
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)
def init_filter(self, timestamp):
self.lat2, self.long2, self.alt2 = self.gps.latitude_deg, self.gps.longitude_deg, self.gps.altitude
roll, pitch, yaw = get_gps_orientation(self.lat1, self.long1,
self.alt1, self.lat2,
self.long2, self.alt2)
yaw = math.atan2(self.lat2 - self.lat1, self.long2 - self.long1)
print(math.degrees(roll), math.degrees(pitch), math.degrees(yaw))
self.filter = GrovesKalmanFilter(initial_roll=roll,
initial_pitch=pitch,
initial_yaw=yaw,
initial_lat=self.lat2,
initial_long=self.long2,
initial_alt=self.alt2,
**constants)
self.imu_t0 = timestamp
self.gps_t0 = timestamp
self.lidar_t0 = timestamp
self.kalman_plots.update_gps(self.gps.latitude_deg,
self.gps.longitude_deg, self.gps.altitude)
self.kalman_plots.update_kalman(self.filter.get_position())
# length = math.sqrt(lat1 ** 2 + long1 ** 2)
lat2 = 0.0003 * math.sin(yaw) + self.lat1
long2 = 0.0003 * math.cos(yaw) + self.long1
self.kalman_plots.initial_compass_plot.updated([self.lat1, lat2],
[self.long1, long2])
def gps_in_range(self):
if not 280 < self.gps.altitude < 310:
self.gps.altitude = 300.0
if -80 < self.gps.longitude_deg < -79.8 and 40.4 < self.gps.latitude_deg < 41 and (
280 < self.gps.altitude < 310 or self.gps.altitude == 0.0):
return True
else:
return False
def update_gps_plot(self):
if self.gps_in_range():
self.kalman_plots.update_gps(self.gps.latitude_deg,
self.gps.longitude_deg,
self.gps.altitude)
return True
else:
return False
def update_epoch(self, timestamp):
if use_filter:
if self.gps.fix and self.gps.latitude is not None and self.received_fix != self.gps.fix:
self.received_fix = True
self.num_recv_from_fix = self.num_received(self.gps)
if self.num_recv_from_fix is not None and self.num_received(
self.gps) - self.num_recv_from_fix == 245:
self.lat1, self.long1, self.alt1 = self.gps.latitude_deg, self.gps.longitude_deg, self.gps.altitude
elif self.filter is None and self.num_recv_from_fix is not None and \
self.gps_in_range() and self.num_received(self.gps) - self.num_recv_from_fix >= 250:
if use_filter:
self.init_filter(timestamp)
elif self.filter is not None and self.num_recv_from_fix is not None:
# if self.num_received(self.gps) % 5 == 0:
if self.update_gps_plot():
if use_filter:
self.filter.gps_updated(timestamp - self.gps_t0,
self.gps.latitude_deg,
self.gps.longitude_deg,
self.gps.altitude)
self.kalman_plots.update_kalman(
self.filter.get_position())
self.gps_t0 = timestamp
lat1, long1, alt1 = self.filter.get_position()
roll, pitch, yaw = self.filter.get_orientation()
yaw = -self.imu.euler.x
# declination = 9, 19
#
# self.declination = \
# (declination[0] + declination[1] / 60) * math.pi / 180
#
# yaw = math.atan2(self.imu.mag.y, self.imu.mag.x)
# yaw += self.declination
# # Correct for reversed heading
# if yaw < 0:
# yaw += 2 * math.pi
# # Check for wrap and compensate
# elif yaw > 2 * math.pi:
# yaw -= 2 * math.pi
# length = math.sqrt(lat1 ** 2 + long1 ** 2)
lat2 = 0.0003 * math.sin(math.radians(yaw)) + lat1
long2 = 0.0003 * math.cos(math.radians(yaw)) + long1
self.kalman_plots.update_compass(
lat1, lat2, long1, long2)
# print(self.gps.latitude_deg, self.gps.longitude_deg, self.gps.altitude)
# if use_filter:
# print(self.filter.get_position(), self.filter.properties.estimated_velocity.T.tolist())
# print(math.degrees(roll), math.degrees(pitch), math.degrees(yaw))
if animate and self.live_plot.should_update():
if self.live_plot.plot() is False:
return False
def update_ins(self, timestamp):
if use_filter and self.filter is not None and self.imu_t0 != timestamp:
self.filter.imu_updated(timestamp - self.imu_t0,
self.imu.linaccel.x, self.imu.linaccel.y,
self.imu.linaccel.z, self.imu.gyro.x,
self.imu.gyro.y, self.imu.gyro.z)
self.kalman_plots.update_kalman(self.filter.get_position())
self.imu_t0 = timestamp
def update_turret(self, timestamp):
if not args.includelidar and self.turret.did_cloud_update():
if self.turret.is_cloud_ready() and self.filter is not None and (
self.num_received(self.gps) - self.num_recv_from_fix >
250):
v = self.filter.get_velocity()
vx = v[0]
vy = v[1]
ang_v = self.filter.get_angular(timestamp - self.lidar_t0)[2]
self.slam.update(timestamp, self.turret.distances,
[vx, vy, ang_v])
self.lidar_t0 = timestamp
if animate:
self.slam_plots.update(self.slam.mapbytes,
self.slam.get_pos())
# print(self.slam.get_pos(), (vx, vy, ang_v))
if animate and not self.live_plot.plot():
return False
# ----- live methods -----
def packet_received(self, timestamp, whoiam, packet):
if self.did_receive(self.imu):
if self.update_ins(timestamp) is False:
return False
elif self.did_receive(self.gps):
if self.update_epoch(timestamp) is False:
return False
print(timestamp)
print(self.gps)
print(self.imu)
elif self.did_receive(self.turret):
if self.update_turret(timestamp) is False:
return False
else:
print(packet)
# elif self.did_receive(self.steering):# and self.steering.goal_reached:
# print(self.steering.current_step)
def loop(self):
if not simulated:
if self.joystick is not None:
if self.steering.calibrated:
if self.joystick.axis_updated("left x"):
self.steering.set_speed(
self.joystick.get_axis("left x"))
elif self.joystick.button_updated(
"A") and self.joystick.get_button("A"):
self.steering.set_position(0)
if self.joystick.button_updated(
"B") and self.joystick.get_button("B"):
self.brakes.pull()
elif self.joystick.button_updated(
"X") and self.joystick.get_button("X"):
self.brakes.release()
elif self.joystick.button_updated(
"Y") and self.joystick.get_button("Y"):
self.record(
"checkpoint", "%s\t%s\t%s" %
(self.checkpoint_num, self.gps.latitude_deg,
self.gps.longitude_deg))
if self.gps.latitude_deg is not None and self.gps.longitude_deg is not None:
print("checkpoint #%s: %3.6f, %3.6f" %
(self.checkpoint_num, self.gps.latitude_deg,
self.gps.longitude_deg))
self.checkpoint_num += 1
print("checkpoint #%s" % self.checkpoint_num)
# elif self.joystick.dpad_updated():
# if self.joystick.dpad[1] == 1:
# self.brakes.set_brake(self.brakes.goal_position + 20)
# elif self.joystick.dpad[1] == -1:
# self.brakes.set_brake(self.brakes.goal_position - 20)
def start(self):
if animate:
self.live_plot.start_time(self.start_time)
# ----- simulator methods -----
def object_packet(self, timestamp):
if self.did_receive(self.imu):
if self.update_ins(timestamp) is False:
return False
elif self.did_receive(self.gps):
if self.update_epoch(timestamp) is False:
return False
elif self.did_receive(self.turret):
if self.update_turret(timestamp) is False:
return False
# def user_packet(self, timestamp, packet):
# if self.did_receive("checkpoint"):
# print(timestamp, packet)
# def command_packet(self, timestamp, packet):
# if self.did_receive(self.steering):
# print(timestamp, packet)
def close(self):
if plots_enabled:
if not animate:
self.static_plot.plot()
self.static_plot.show()
else:
if simulated:
if not self.error_signalled:
print("Finished!")
self.live_plot.freeze_plot()
self.live_plot.close()
if args.computeslam:
self.slam.make_image()
class AnimationExample(Robot):
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 key_press(self, event):
if event.key == " ":
self.toggle_pause()
self.plotter.toggle_pause()
def receive_gps(self, timestamp, packet, packet_type):
if self.gps.is_position_valid():
self.gps_plot.append(self.gps.latitude_deg, self.gps.longitude_deg)
status = self.plotter.plot()
if status is not None:
return status
def init_compass(self, packet):
self.compass_angle = math.radians(float(packet)) - math.pi / 2
def offset_angle(self):
if self.start_angle is None:
self.start_angle = -self.imu.euler.z
return (-self.imu.euler.z + self.start_angle -
self.compass_angle) % (2 * math.pi)
def compass_coords(self, angle, length=0.0003):
lat2 = length * math.sin(angle) + self.gps.latitude_deg
long2 = length * math.cos(angle) + self.gps.longitude_deg
return lat2, long2
def receive_imu(self, timestamp, packet, packet_type):
if self.gps.is_position_valid():
angle = self.offset_angle()
lat2, long2 = self.compass_coords(angle)
self.compass_plot.update([self.gps.latitude_deg, lat2],
[self.gps.longitude_deg, long2])
self.plotter.draw_text(self.accuracy_check_plot,
"%0.4f" % angle,
self.gps.latitude_deg,
self.gps.longitude_deg,
text_name="angle text")
if self.sticky_compass_skip > 0 and self.sticky_compass_counter % self.sticky_compass_skip == 0:
lat2, long2 = self.compass_coords(angle, length=0.0001)
self.sticky_compass_plot.append(self.gps.latitude_deg,
self.gps.longitude_deg)
self.sticky_compass_plot.append(lat2, long2)
self.sticky_compass_plot.append(self.gps.latitude_deg,
self.gps.longitude_deg)
self.sticky_compass_counter += 1
def received(self, timestamp, whoiam, packet, packet_type):
if self.did_receive("initial compass"):
self.init_compass(packet)
print("compass value:", packet)
def loop(self):
if self.is_paused:
status = self.plotter.plot()
if status is not None:
return status
def close(self, reason):
if reason == "done":
self.plotter.freeze_plot()
self.plotter.close()
print(self.dt())