class Vision: chassis: SwerveChassis # NOTE: x and y are relative to the robot co-ordinate system, not the camera @property def fiducial_x(self) -> float: return self.fiducial_x_entry.getDouble(0.0) @property def fiducial_y(self) -> float: return self.fiducial_y_entry.getDouble(0.0) @property def fiducial_time(self) -> float: return self.fiducial_time_entry.getDouble(-1.0) @property def ping_time(self) -> float: return self.ping_time_entry.getDouble(0.0) @ping_time.setter def ping_time(self, value: float) -> None: self.ping_time_entry.setDouble(value) @property def raspi_pong_time(self) -> float: return self.raspi_pong_time_entry.getDouble(0.0) @property def rio_pong_time(self) -> float: return self.rio_pong_time_entry.getDouble(0.0) @property def latency(self) -> float: return self.latency_entry.getDouble(0.0) @latency.setter def latency(self, value: float) -> None: self.latency_entry.setDouble(value) @property def processing_time(self) -> float: return self.processing_time_entry.getDouble(0.0) @processing_time.setter def processing_time(self, value: float) -> None: self.processing_time_entry.setDouble(value) def __init__(self) -> None: self.last_pong = time.monotonic() # 50Hz control loop for 2 seconds self.odometry: Deque[Odometry] = deque(maxlen=50 * 2) self.ntinst = NetworkTablesInstance() if hal.isSimulation(): self.ntinst.startTestMode(server=False) else: self.ntinst.startClient("10.47.74.6") # Raspberry pi's IP self.ntinst.setUpdateRate( 1) # ensure our flush calls flush immediately self.fiducial_x_entry = self.ntinst.getEntry("/vision/fiducial_x") self.fiducial_y_entry = self.ntinst.getEntry("/vision/fiducial_y") self.fiducial_time_entry = self.ntinst.getEntry( "/vision/fiducial_time") self.ping_time_entry = self.ntinst.getEntry("/vision/ping") self.raspi_pong_time_entry = self.ntinst.getEntry("/vision/raspi_pong") self.rio_pong_time_entry = self.ntinst.getEntry("/vision/rio_pong") self.latency_entry = self.ntinst.getEntry("/vision/clock_offset") self.processing_time_entry = self.ntinst.getEntry( "/vision/processing_time") self.camera_entry = self.ntinst.getEntry("/vision/game_piece") def execute(self) -> None: """Store the current odometry in the queue. Allows projection of target into current position.""" self.odometry.appendleft( Odometry( self.chassis.odometry_x, self.chassis.odometry_y, self.chassis.imu.getAngle(), time.monotonic(), )) self.ping() self.pong() vision_time = self.fiducial_time + self.latency self.processing_time = time.monotonic() - vision_time self.ntinst.flush() @property def fiducial_in_sight(self) -> bool: return time.monotonic() - (self.fiducial_time + self.latency) < 0.1 def get_fiducial_position(self) -> Tuple[float, float, float]: """Return the position of the retroreflective fiducials relative to the current robot pose.""" vision_time = self.fiducial_time + self.latency vision_delta_x, vision_delta_y, vision_delta_heading = self._get_pose_delta( vision_time) x = self.fiducial_x - vision_delta_x y = self.fiducial_y - vision_delta_y return x, y, vision_delta_heading def _get_pose_delta(self, t: float) -> Tuple[float, float, float]: """Search the stored odometry and return the position difference between now and the specified time.""" current = previous = self.odometry[0] for odom in self.odometry: if odom.t >= t: previous = odom else: break x = current.x - previous.x y = current.y - previous.y # Rotate to the robot frame of reference # Use the previous heading - that's where we were when the picture was taken heading = previous.heading robot_x, robot_y = rotate_vector(x, y, -heading) return robot_x, robot_y, current.heading - heading def ping(self) -> None: """Send a ping to the RasPi to determine the connection latency.""" self.ping_time = time.monotonic() def pong(self) -> None: """Receive a pong from the RasPi to determine the connection latency.""" if abs(self.rio_pong_time - self.last_pong) > 1e-4: # Floating point comparison alpha = 0.9 # Exponential averaging self.latency = (1 - alpha) * self.latency + alpha * ( self.rio_pong_time - self.raspi_pong_time) self.last_pong = self.rio_pong_time def use_hatch(self) -> None: """Switch to the hatch camera.""" self.camera_entry.setDouble(0) def use_cargo(self) -> None: """Switch to the cargo camera.""" self.camera_entry.setDouble(1)
if abs(ping_time - old_ping_time) > 0.00000001: raspi_pong.setNumber(time.monotonic()) rio_pong.setNumber(ping_time) old_ping_time = ping_time game_piece = entry_game_piece.getNumber(0) fiducial_time = time.monotonic() sink = hatch_sink if game_piece == 0 else cargo_rocket_sink entry_camera.setBoolean(bool(game_piece)) frame_time, frame = sink.grabFrameNoTimeout(image=frame) if frame_time == 0: print(sink.getError(), file=sys.stderr) source.notifyError(sink.getError()) outtake = False percent = math.nan else: image, dist, offset = getRetroPos(frame, True, hsv, mask) source.putFrame(image) if ( not math.isnan(dist) and not dist < 0.6 and not dist > 3 and not abs(offset) > 2 ): if game_piece == 1: dist *= -1 offset *= -1 entry_dist.setNumber(dist) entry_offset.setNumber(offset) entry_fiducial_time.setNumber(fiducial_time) ntinst.flush()