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