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 RoboQuasar(Robot):
def __init__(self,
enable_plotting,
map_set_name,
initial_compass=None,
animate=True,
enable_cameras=True,
use_log_file_maps=True,
show_cameras=None,
day_mode=False,
only_cameras=False,
enable_pid=True,
enable_kalman=True):
# ----- initialize robot objects -----
self.gps = GPS()
self.imu = IMU()
self.steering = Steering()
self.brakes = Brakes()
self.underglow = Underglow()
super(RoboQuasar, self).__init__(
self.gps,
self.imu,
self.steering,
self.brakes,
self.underglow,
)
self.manual_mode = True
# ----- init CV classes ------
if show_cameras is None:
show_cameras = enable_plotting
self.only_cameras = only_cameras
if self.only_cameras:
self.enable_cameras = True
self.camera = Camera("rightcam",
enabled=enable_cameras,
show=show_cameras)
self.day_mode = day_mode
self.pipeline = Pipeline(self.camera,
self.day_mode,
enabled=False,
separate_read_thread=False)
# ----- init filters and controllers -----
# position state message (in or out of map)
self.position_state = ""
self.prev_pos_state = self.position_state
# init controller
self.map_set_name = map_set_name
checkpoint_map_name, inner_map_name, outer_map_name, map_dir = map_sets[
map_set_name]
self.init_checkpoints = [
0, # 0, hill 1
17, # 1, hill 2
121, # 2, hill 3
127, # 3, hill 4
143, # 4, hill 5
]
self.init_offset = 12
self.map_manipulator = MapManipulator(
checkpoint_map_name,
map_dir,
inner_map_name,
outer_map_name,
offset=2,
init_indices=self.init_checkpoints)
self.pid = None
self.enable_pid = enable_pid
self.gps_disabled = False
self.prev_gps_disabled = False
self.disabled_gps_checkpoints = [
(0, 12),
# (13, 17),
(130, len(self.map_manipulator) - 1)
]
self.angle_filter = AngleManipulator(initial_compass)
self.use_log_file_maps = use_log_file_maps
# ----- filtered outputs -----
self.imu_heading_guess = 0.0
self.steering_angle = 0.0
self.map_heading = 0.0
self.gps_corrected_lat = 0.0
self.gps_corrected_long = 0.0
self.left_limit = None
self.map_lat = 0.0
self.map_long = 0.0
self.kalman = None
self.gps_t0 = 0.0
self.imu_t0 = 0.0
# ----- dashboard (parameters to tweak) -----
self.map_weight = 0.15
self.kp = 0.9
self.kd = 0.0
self.ki = 0.0
self.angle_threshold = math.radians(10)
self.percent_threshold = 0.4
self.enable_drift_correction = False
self.enable_kalman = enable_kalman
# ----- drift correction -----
self.lat_drift = 0.0
self.long_drift = 0.0
self.drift_corrected = False
self.lat_drift_data = []
self.long_drift_data = []
self.bearing_data = []
# ----- status variables -----
self.gps_within_map = True
self.prev_within_map_state = True
self.prev_gps_state = False
self.uncertainty_condition = False
self.prev_pipeline_value_state = False
self.prev_index = -1
# ----- link callbacks -----
self.link_object(self.gps, self.receive_gps)
self.link_object(self.imu, self.receive_imu)
self.link_reoccuring(0.008, self.steering_event)
self.link_reoccuring(0.01, self.brake_ping)
# ----- init plots -----
self.quasar_plotter = RoboQuasarPlotter(
animate, enable_plotting, self.enable_kalman,
self.map_manipulator.map, self.map_manipulator.inner_map,
self.map_manipulator.outer_map, self.key_press, self.map_set_name,
False)
def start(self):
# extract camera file name from current log file name
file_name = self.get_path_info("file name no extension").replace(
";", "_")
directory = self.get_path_info("input dir")
# start cameras and pipelines
self.open_cameras(file_name, directory, "avi")
self.pipeline.start()
# record important data
self.record("map set", self.map_set_name)
self.record_compass()
if self.day_mode is not None:
self.record("pipeline mode", str(int(self.day_mode)))
self.debug_print("Using %s mode pipelines" %
("day" if self.day_mode else "night"),
ignore_flag=True)
self.brakes.unpause_pings()
# self.left_limit = abs(self.steering.left_limit_angle / 3)
self.pid = PID(
self.kp, self.kd, self.ki
) # self.steering.left_limit_angle, self.steering.right_limit_angle)
print(self.dt())
# ----- camera initialization -----
def open_cameras(self, log_name, directory, file_format):
# create log name
# cam_name = "%s%s.%s" % (log_name, self.camera.name, file_format)
cam_name = "%s.%s" % (log_name, file_format)
if self.logger is None:
record = True
else:
record = self.logger.is_open()
# launch a live camera if the robot is live, otherwise launch a video
if self.is_live:
status = self.camera.launch_camera(cam_name,
directory,
record,
capture_number=1)
if status is not None:
return status
else:
self.camera.launch_video(cam_name, directory)
# ----- sensor received -----
def receive_gps(self, timestamp, packet, packet_type):
is_valid = self.gps.is_position_valid()
if is_valid != self.prev_gps_state:
self.prev_gps_state = is_valid
if is_valid:
print("GPS has locked on:")
print(self.gps)
else:
print("WARNING. GPS has lost connection!!!")
if is_valid:
if not self.map_manipulator.is_initialized():
self.map_manipulator.initialize(self.gps.latitude_deg,
self.gps.longitude_deg)
if self.enable_kalman and self.kalman is not None:
self.kalman.gps_updated(timestamp - self.gps_t0,
self.gps.latitude_deg,
self.gps.longitude_deg,
self.gps.altitude)
position = self.kalman.get_position()
self.quasar_plotter.kalman_recomputed_plot.append(
position[0], position[1])
self.gps_t0 = timestamp
self.gps_corrected_lat = self.gps.latitude_deg
self.gps_corrected_long = self.gps.longitude_deg
if self.drift_corrected:
if self.enable_drift_correction:
self.gps_corrected_lat += self.lat_drift
self.gps_corrected_long += self.long_drift
outer_state = self.map_manipulator.point_inside_outer_map(
self.gps_corrected_lat, self.gps_corrected_long)
inner_state = self.map_manipulator.point_outside_inner_map(
self.gps_corrected_lat, self.gps_corrected_long)
self.gps_within_map = outer_state and inner_state
if self.prev_within_map_state != self.gps_within_map and not self.gps_within_map:
print(
"WARNING. Drift correction has been applied but GPS is still out of the map."
)
self.prev_within_map_state = self.gps_within_map
self.quasar_plotter.update_map_colors(timestamp, outer_state,
inner_state)
self.map_heading = self.map_manipulator.get_goal_angle(
self.gps_corrected_lat, self.gps_corrected_long)
self.map_lat, self.map_long = self.map_manipulator.map[
self.map_manipulator.current_index]
self.gps_corrected_lat = self.gps_corrected_lat * (
1 - self.map_weight) + self.map_lat * self.map_weight
self.gps_corrected_long = self.gps_corrected_long * (
1 - self.map_weight) + self.map_long * self.map_weight
if self.map_manipulator.current_index > self.init_checkpoints[
1]:
self.angle_filter.update_bearing(self.gps_corrected_lat,
self.gps_corrected_long)
self.quasar_plotter.corrected_gps_plot.append(
self.gps_corrected_lat, self.gps_corrected_long)
if self.gps.fix:
status = self.quasar_plotter.update_gps_plot(
timestamp, self.gps.latitude_deg, self.gps.longitude_deg)
if status is not None:
return status
def receive_imu(self, timestamp, packet, packet_type):
if self.only_cameras:
self.check_pipeline()
self.update_steering()
return
if (self.angle_filter.initialized and self.drift_corrected
and self.gps.is_position_valid()
and self.map_manipulator.is_initialized()):
self.imu_heading_guess = self.angle_filter.offset_angle(
self.imu.euler.z)
# assign steering angle from IMU regardless of the pipeline,
# in case the pipeline finds it's too close but sees the buggy is straight. In this case, the pipeline
# would not assign a new angle
self.prev_gps_disabled = self.gps_disabled
self.gps_disabled = False
goal_checkpoint = None
current_checkpoint = None
for checkpoint_set in self.disabled_gps_checkpoints:
if checkpoint_set[
0] <= self.map_manipulator.current_index <= checkpoint_set[
1]:
self.gps_disabled = True
current_checkpoint = checkpoint_set[0]
goal_checkpoint = checkpoint_set[1]
if self.prev_gps_disabled != self.gps_disabled:
if self.gps_disabled:
self.debug_print(
"Ignoring GPS, driving straight until:",
checkpoint_set[1],
ignore_flag=True)
else:
self.debug_print(
"Reapplying GPS, current index:",
self.map_manipulator.current_index,
ignore_flag=True)
break
if self.enable_kalman:
if self.kalman is not None:
self.kalman.imu_updated(timestamp - self.imu_t0,
self.imu.accel.x, self.imu.accel.y,
self.imu.accel.z, self.imu.gyro.x,
self.imu.gyro.y, self.imu.gyro.z)
self.imu_t0 = timestamp
position = self.kalman.get_position()
current_lat = position[0]
current_long = position[1]
# orientation = self.kalman.get_orientation()
angle_error = self.map_manipulator.update(
current_lat,
current_long,
self.imu_heading_guess #orientation[2]
)
goal_lat = self.map_manipulator.goal_lat
goal_long = self.map_manipulator.goal_long
else:
return
else:
if self.gps_disabled:
# self.check_pipeline()
current_lat, current_long = self.map_manipulator.map[
current_checkpoint]
angle_error = self.map_manipulator.update(
current_lat,
current_long,
self.imu_heading_guess,
goal_num=goal_checkpoint)
goal_lat, goal_long = self.map_manipulator.map[
goal_checkpoint]
else:
angle_error = self.map_manipulator.update(
self.gps_corrected_lat, self.gps_corrected_long,
self.imu_heading_guess)
current_lat = self.gps_corrected_lat
current_long = self.gps_corrected_long
goal_lat = self.map_manipulator.goal_lat
goal_long = self.map_manipulator.goal_long
if self.map_manipulator.current_index != self.prev_index:
print("Current map location:",
self.map_manipulator.current_index)
self.prev_index = self.map_manipulator.current_index
if self.enable_pid:
self.steering_angle = self.pid.update(angle_error, self.dt())
else:
self.steering_angle = angle_error
self.quasar_plotter.update_indicators(
(current_lat, current_long),
(goal_lat, goal_long),
self.imu_heading_guess,
self.steering_angle,
)
self.update_steering()
def received(self, timestamp, whoiam, packet, packet_type):
if self.did_receive("initial compass"):
self.angle_filter.init_compass(packet)
self.debug_print("initial offset: %0.4f rad" %
self.angle_filter.compass_angle,
ignore_flag=True)
elif self.did_receive("maps"):
if self.use_log_file_maps:
if "," in packet:
checkpoint_map_name, inner_map_name, outer_map_name, map_dir = packet.split(
",")
else:
checkpoint_map_name, inner_map_name, outer_map_name, map_dir = map_sets[
packet]
self.quasar_plotter.plot_image(packet)
self.map_manipulator.init_maps(checkpoint_map_name, map_dir,
inner_map_name, outer_map_name)
self.quasar_plotter.update_maps(self.map_manipulator.map,
self.map_manipulator.inner_map,
self.map_manipulator.outer_map)
elif self.did_receive("drift correction"):
if int(packet) == 1:
# self.calibrate_with_checkpoint()
self.calibrate_imu()
# elif int(packet) == 2:
elif self.did_receive("pipeline mode"):
day_mode = bool(int(packet))
self.pipeline.day_mode = day_mode
self.debug_print("Switching to %s mode" %
("day" if day_mode else "night"),
ignore_flag=True)
# ----- autonomous steering -----
def update_steering(self):
if (self.angle_filter.initialized
and self.drift_corrected) or self.only_cameras:
if self.left_limit is not None and self.steering_angle > self.left_limit: # never turn left too much
self.steering_angle = self.left_limit
if not self.manual_mode:
self.steering.set_position(self.steering_angle)
def check_pipeline(self):
if self.pipeline.safety_value > self.percent_threshold:
if not self.prev_pipeline_value_state:
print("Pipeline detected edge above threshold: %0.4f, %0.4f" %
(self.pipeline.safety_value, self.pipeline.line_angle))
self.prev_pipeline_value_state = True
if not (0 < self.pipeline.line_angle <
self.angle_threshold) or self.only_cameras:
self.steering_angle = -self.pipeline.line_angle
print("CV angle: %0.4f" % self.pipeline.line_angle)
else:
self.steering_angle = 0
# steer more if the curb is closer
self.steering_angle -= self.pipeline.safety_value * abs(
self.steering.right_limit_angle)
elif self.prev_pipeline_value_state:
print(
"IMU is taking over steering. Steering: %s, IMU: %s, Map: %s" %
(self.steering_angle, self.imu_heading_guess,
self.map_heading))
self.prev_pipeline_value_state = False
def calibrate_with_checkpoint(self):
if self.gps.is_position_valid():
self.record("drift correction", 1)
locked_lat, locked_long, locked_index = self.map_manipulator.lock_onto_map(
self.gps.latitude_deg, self.gps.longitude_deg, True)
self.lat_drift = locked_lat - self.gps.latitude_deg
self.long_drift = locked_long - self.gps.longitude_deg
# next_lat, next_long = self.map_manipulator.map[locked_index + self.init_offset]
# bearing = self.angle_filter.bearing_to(locked_lat, locked_long, next_lat, next_long)
# bearing = math.atan2(next_long - locked_long,
# next_lat - locked_lat)
# bearing %= 2 * math.pi
# self.angle_filter.init_compass(bearing)
# self.record_compass()
self.debug_print("\n=====\n"
"\tCorrecting GPS"
"\tDrift: (%0.8f, %0.8f)\n"
"=====" % (self.lat_drift, self.long_drift),
ignore_flag=True)
# self.debug_print("\tBearing: %0.6frad, %0.6fdeg\n"
# "=====" % (bearing, math.degrees(bearing)), ignore_flag=True)
if self.drift_corrected:
self.debug_print("\tDrift already corrected", ignore_flag=True)
self.drift_corrected = True
else:
self.debug_print("Invalid GPS. Ignoring", ignore_flag=True)
def calibrate_imu(self):
if self.gps.is_position_valid():
self.record("drift correction", 2)
locked_lat, locked_long, locked_index = self.map_manipulator.lock_onto_map(
self.gps.latitude_deg, self.gps.longitude_deg, True)
next_lat, next_long = self.map_manipulator.map[locked_index +
self.init_offset]
bearing = math.atan2(next_long - locked_long,
next_lat - locked_lat)
bearing %= 2 * math.pi
self.angle_filter.init_compass(bearing)
self.record_compass()
self.debug_print("\n=====\n" "\tCorrecting IMU", ignore_flag=True)
self.debug_print("\tBearing: %0.6frad, %0.6fdeg\n"
"=====" % (bearing, math.degrees(bearing)),
ignore_flag=True)
if self.drift_corrected:
self.debug_print("\tIMU already corrected", ignore_flag=True)
self.drift_corrected = True
if self.enable_kalman and self.kalman is None:
self.kalman = GrovesKalmanFilter(initial_roll=0.0,
initial_pitch=0.0,
initial_yaw=bearing,
initial_lat=locked_lat,
initial_long=locked_long,
initial_alt=self.gps.altitude,
**constants)
else:
self.debug_print("Invalid GPS. Ignoring", ignore_flag=True)
def record_compass(self):
if self.angle_filter.initialized:
self.record("initial compass",
self.angle_filter.compass_angle_packet)
self.debug_print("initial offset: %0.4f rad" %
self.angle_filter.compass_angle,
ignore_flag=True)
# ----- manual steering -----
def loop(self):
if self.is_paused:
status = self.quasar_plotter.check_paused(self.dt())
if status is not None:
return status
self.update_joystick()
def update_joystick(self):
if self.joystick is not None:
if self.steering.calibrated and self.manual_mode:
if self.joystick.axis_updated(
"left x") or self.joystick.axis_updated("left y"):
mag = math.sqrt(
self.joystick.get_axis("left y")**2 +
self.joystick.get_axis("left x")**2)
if mag > 0.5:
angle = math.atan2(self.joystick.get_axis("left y"),
self.joystick.get_axis("left x"))
angle -= math.pi / 2
if angle < -math.pi:
angle += 2 * math.pi
self.steering.set_position(angle / 4)
elif self.joystick.button_updated(
"A") and self.joystick.get_button("A"):
self.steering.calibrate()
elif self.joystick.dpad_updated():
if self.joystick.dpad[1] != 0:
self.steering.set_position(0)
if self.joystick.button_updated("X") and self.joystick.get_button(
"X"):
self.manual_mode = not self.manual_mode
self.debug_print("Manual control enabled" if self.manual_mode
else "Autonomous mode enabled",
ignore_flag=True)
elif self.joystick.button_updated("L"):
if self.joystick.get_button("L"):
self.brakes.release()
self.underglow.signal_release()
else:
self.brakes.pull()
self.underglow.signal_brake()
elif self.joystick.button_updated(
"R") and self.joystick.get_button("R"):
self.brakes.toggle()
if self.brakes.engaged:
self.underglow.signal_brake()
else:
self.underglow.signal_release()
elif self.joystick.button_updated(
"Y") and self.joystick.get_button("Y"):
self.calibrate_with_checkpoint()
elif self.joystick.button_updated(
"B") and self.joystick.get_button("B"):
self.calibrate_imu()
@staticmethod
def my_round(x, d=0):
p = 10**d
return float(math.floor((x * p) + math.copysign(0.5, x))) / p
@staticmethod
def sigmoid(x): # modified sigmoid. -1...1
return (-1 / (1 + math.exp(-x)) + 0.5) * 2
# ----- extra events -----
def brake_ping(self):
self.brakes.ping()
def steering_event(self):
if self.steering.calibrated and self.manual_mode:
joy_val = self.joystick.get_axis("right x")
delta_step = int(-self.my_round(8 * joy_val))
if abs(delta_step) > 0:
self.steering.change_step(delta_step)
def key_press(self, event):
if event.key == " ":
self.toggle_pause()
if isinstance(self.quasar_plotter.plotter, LivePlotter):
self.quasar_plotter.plotter.toggle_pause()
self.pipeline.paused = not self.pipeline.paused
elif event.key == "q":
self.quasar_plotter.close("exit")
self.close("exit")
elif event.key == "[":
if self.parser is not None:
print(self.parser.index, end=" -> ")
self.parser.index += 500
print(self.parser.index, self.current_timestamp)
elif event.key == "]":
if self.parser is not None:
print(self.parser.index, end=" -> ")
self.parser.index += 5000
print(self.parser.index, self.current_timestamp)
self.pid.error_sum = 0
def close(self, reason):
if reason != "done":
self.brakes.pull()
self.debug_print("!!EMERGENCY BRAKE!!", ignore_flag=True)
else:
self.brakes.pause_pings()
self.quasar_plotter.close(reason)
self.pipeline.close()
print("Ran for %0.4fs" % self.dt())
class CameraGuidanceTest(Robot):
def __init__(self, enable_cameras=True, show_cameras=False):
# ----- initialize robot objects -----
self.steering = Steering()
self.brakes = Brakes()
self.underglow = Underglow()
super(CameraGuidanceTest, self).__init__(
self.steering,
self.brakes,
self.underglow,
)
self.manual_mode = True
# ----- init CV classes ------
self.left_camera = Camera("leftcam",
enabled=enable_cameras,
show=show_cameras)
self.right_camera = Camera("rightcam",
enabled=False,
show=show_cameras)
self.left_pipeline = Pipeline(self.left_camera,
separate_read_thread=False)
self.right_pipeline = Pipeline(self.right_camera,
separate_read_thread=False)
# ----- init filters and controllers
# position state message (in or out of map)
self.position_state = ""
self.prev_pos_state = self.position_state
# init controller
self.steering_angle = 0.0
self.link_reoccuring(0.008, self.steering_event)
def start(self):
# extract camera file name from current log file name
file_name = self.get_path_info("file name no extension").replace(
";", "_")
directory = self.get_path_info("input dir")
# start cameras and pipelines
self.open_cameras(file_name, directory, "mp4")
self.left_pipeline.start()
self.right_pipeline.start()
def open_cameras(self, log_name, directory, file_format):
# create log name
left_cam_name = "%s%s.%s" % (log_name, self.left_camera.name,
file_format)
right_cam_name = "%s%s.%s" % (log_name, self.right_camera.name,
file_format)
if self.logger is None:
record = True
else:
record = self.logger.is_open()
# launch a live camera if the robot is live, otherwise launch a video
if self.is_live:
status = self.left_camera.launch_camera(left_cam_name,
directory,
record,
capture_number=1)
if status is not None:
return status
status = self.right_camera.launch_camera(right_cam_name,
directory,
record,
capture_number=2)
if status is not None:
return status
else:
self.left_camera.launch_video(left_cam_name,
directory,
start_frame=0)
self.right_camera.launch_video(right_cam_name, directory)
def received(self, timestamp, whoiam, packet, packet_type):
self.left_pipeline.update_time(self.dt())
def loop(self):
status = self.update_pipeline()
if status is not None:
return status
self.update_joystick()
def steering_event(self):
self.brakes.ping()
if self.steering.calibrated and self.manual_mode:
# if self.joystick.get_axis("ZR") >= 1.0:
joy_val = self.joystick.get_axis("right x")
if abs(joy_val) > 0.0:
offset = math.copysign(0.3, joy_val)
joy_val -= offset
delta_step = self.my_round(16 * self.sigmoid(10.0 * joy_val))
self.steering.change_step(delta_step)
@staticmethod
def my_round(x, d=0):
p = 10**d
return float(math.floor((x * p) + math.copysign(0.5, x))) / p
@staticmethod
def sigmoid(x): # modified sigmoid. -1...1
return (-1 / (1 + math.exp(-x)) + 0.5) * 2
def update_joystick(self):
if self.joystick is not None:
if self.steering.calibrated and self.manual_mode:
if self.joystick.axis_updated(
"left x") or self.joystick.axis_updated("left y"):
mag = math.sqrt(
self.joystick.get_axis("left y")**2 +
self.joystick.get_axis("left x")**2)
if mag > 0.5:
angle = math.atan2(self.joystick.get_axis("left y"),
self.joystick.get_axis("left x"))
angle -= math.pi / 2
if angle < -math.pi:
angle += 2 * math.pi
self.steering.set_position(angle / 4)
elif self.joystick.button_updated(
"A") and self.joystick.get_button("A"):
self.steering.calibrate()
elif self.joystick.dpad_updated():
# if self.joystick.dpad[0] != 0:
# self.steering.change_position(-self.joystick.dpad[0] * 10)
if self.joystick.dpad[1] != 0:
self.steering.set_position(0)
if self.joystick.button_updated("X") and self.joystick.get_button(
"X"):
self.manual_mode = not self.manual_mode
print("Manual control enabled" if self.
manual_mode else "Autonomous mode enabled")
elif self.joystick.button_updated("L"):
if self.joystick.get_button("L"):
self.brakes.release()
self.underglow.signal_release()
# self.delay_function(1.5, self.dt(), self.underglow.rainbow_cycle)
else:
self.brakes.pull()
self.underglow.signal_brake()
# self.delay_function(1.5, self.dt(), self.underglow.rainbow_cycle)
elif self.joystick.button_updated(
"R") and self.joystick.get_button("R"):
self.brakes.toggle()
if self.brakes.engaged:
self.underglow.signal_brake()
# self.delay_function(1.5, self.dt(), self.underglow.rainbow_cycle)
else:
self.underglow.signal_release()
# self.delay_function(1.5, self.dt(), self.underglow.rainbow_cycle)
def update_pipeline(self):
if not self.manual_mode:
if self.left_pipeline.did_update():
value = self.left_pipeline.safety_value
if value > self.left_pipeline.safety_threshold:
new_angle = self.steering_angle + self.steering.left_limit_angle * value
print(new_angle)
self.steering.set_position(new_angle)
self.gps_imu_control_enabled = False
else:
self.gps_imu_control_enabled = True
if self.right_pipeline.did_update():
value = self.right_pipeline.safety_value
if value > self.right_pipeline.safety_threshold:
new_angle = self.steering_angle + self.steering.right_limit_angle * value
self.steering.set_position(new_angle)
self.gps_imu_control_enabled = False
else:
self.gps_imu_control_enabled = True
if self.left_pipeline.status is not None:
return self.left_pipeline.status
if self.left_pipeline.did_pause():
self.toggle_pause()
if self.right_pipeline.status is not None:
return self.right_pipeline.status
def close(self, reason):
if reason != "done":
self.brakes.pull()
self.debug_print("!!EMERGENCY BRAKE!!", ignore_flag=True)
self.left_pipeline.close()
self.right_pipeline.close()
print("Ran for %0.4fs" % self.dt())