def __init__(self): self.gps = GPS(enabled=False) self.imu = IMU(enabled=False) self.steering = Steering(enabled=False) self.brakes = Brakes() super(BrakeTest, self).__init__(self.gps, self.imu, self.steering, self.brakes, log_data=False, debug_prints=True)
def __init__(self): self.gps = GPS() self.imu = IMU() self.lidar = LidarTurret() self.steering = Steering() self.brakes = Brakes() self.underglow = Underglow() super(FilePrinter, self).__init__(None, "rolls/2017_Feb_24", self.gps, self.imu, self.lidar, self.steering, self.brakes, self.underglow)
class BrakeTest(RobotRunner): def __init__(self): self.gps = GPS(enabled=False) self.imu = IMU(enabled=False) self.steering = Steering(enabled=False) self.brakes = Brakes() super(BrakeTest, self).__init__(self.gps, self.imu, self.steering, self.brakes, log_data=False, debug_prints=True) def loop(self): input_value = input("> ") if input_value == "-": self.brakes.set_brake(input_value) else: try: brake_value = int(input_value) if abs(brake_value - self.brakes.position) >= 20: response = input( "input value difference is more than 20, are you sure you want to proceed? (y/n)" ) if response == "y": self.brakes.set_brake(brake_value) else: print("skipping") except ValueError: pass self.brakes.set_brake(input_value)
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 __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)
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())
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)
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 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())