payload.modes.PayloadCornerNavigateMode

PayloadCornerNavigateMode: starting from inside the alternating-border DLZ, drive out to a tape edge, align with it, then follow the border in the configured travel direction (cw / ccw) until reaching the first corner.

Phase sequence:

DRIVE_OUT → TURN_TO_CENTER → LINE_FOLLOW → TAPE_ALIGN → DONE

DRIVE_OUT

Drive forward at drive_out_speed_mps. Subscribes to the payload camera directly (mirroring PayloadRetreatMode) and inspects only the bottom drive_out_strip_frac of the frame, so the line is recognised when it is physically under the payload, not when it first appears far ahead.

Two sub-states with hysteresis:

seeking_tape – no colour ever seen yet. Drive forward until colour A

OR colour B pixel count crosses drive_out_min_pixels for detect_frames consecutive frames; record the dominant colour and advance to crossing_tape.

crossing_tape – currently driving over the tape. Keep driving forward

until A AND B both fall below drive_out_min_pixels for detect_frames consecutive frames — that means we have crossed the line and are now on the far side. Stop and advance to TURN_TO_CENTER.

TURN_TO_CENTER

Rotate in place using the same search/turn/lock pattern as TAPE_ALIGN, but on the combined border-tape centroid in the bottom-third crop. While no tape is visible, rotate in the nominal search direction for the configured travel direction. Once tape is visible, turn toward the centroid until it stays within center_tol_px of crop center for center_stable_frames consecutive frames; the dominant colour observed during the lock seeds LINE_FOLLOW. Caps rotation at max_turn_to_center_rad as a safety bail-out.

LINE_FOLLOW

Proportional-steering line follower on the currently-tracked colour’s centroid within a wide bottom strip. Detects A↔B transitions (using the same 1.5× dominance rule as PayloadColorSquareNode) and stops on the first transition that matches the corner signature for the current direction:

direction=”ccw” : B→A = corner (red→blue with default YAML) direction=”cw” : A→B = corner (blue→red with default YAML)

_prev_color is seeded from TURN_TO_CENTER’s last dominant observation, falling back to the colour caught during DRIVE_OUT.

Attributes

_COLOR_DOMINANCE_RATIO

Classes

PayloadCornerNavigateParams
PayloadCornerNavigateMode

Module Contents

_COLOR_DOMINANCE_RATIO = 1.5

class PayloadCornerNavigateParams

Bases: vehicle_common.mode_loader.ParamsBase

direction: str = 'ccw'

ccw_lower_hsv: list[int] = (0, 80, 80)

ccw_upper_hsv: list[int] = (10, 255, 255)

cw_lower_hsv: list[int] = (85, 120, 60)

cw_upper_hsv: list[int] = (140, 255, 255)

drive_out_speed_mps: float = 0.12

detect_frames: int = 3

max_drive_out_s: float = 30.0

drive_out_strip_frac: float = 0.3

drive_out_min_pixels: int = 150

compressed_image: bool = False

align_angular_speed: float = 0.4

center_tol_px: float = 30.0

center_min_pixels: int = 150

center_stable_frames: int = 3

max_turn_to_center_rad: float = 6.283185307179586

line_follow_speed_mps: float = 0.1

line_follow_strip_frac: float = 0.4

line_follow_min_pixels: int = 50

k_lat: float = 0.003

k_d: float = 0.001

max_angular: float = 0.5

tape_align_lower_hsv: list[int] = (0, 0, 0)

tape_align_upper_hsv: list[int] = (180, 255, 60)

tape_align_angular_speed: float = 0.6

tape_align_center_tol_px: float = 20.0

tape_align_min_pixels: int = 100

tape_align_stable_frames: int = 3

class PayloadCornerNavigateMode

Bases: vehicle_common.mode.Mode

mission_target = 'payload'

transition_labels = ('complete',)

requires_camera = True

initialize(node: rclpy.node.Node, vehicle: payload.payload.Payload, params: PayloadCornerNavigateParams) → None

_corner_transition(prev: str, curr: str) → bool

True when the A↔B transition is a corner for the current travel direction.

on_enter() → None

on_update(time_delta: float) → None

check_status() → str

on_exit() → None

_image_cb(msg) → None

_decode_image() → numpy.ndarray | None

_primary_contour_mask(mask: numpy.ndarray, *, min_area_px: int = 0) → numpy.ndarray

Return a new mask containing one preferred contour.

If multiple contours clear min_area_px, choose the one with the smallest bounding-box width. If only one contour clears the threshold, keep that contour. If no contour clears the threshold, fall back to the largest contour by area so low-signal frames behave like the old path.

_dlz_roi_mask(bgr: numpy.ndarray) → numpy.ndarray

_threshold_with_roi(hsv: numpy.ndarray, lower: numpy.ndarray, upper: numpy.ndarray, roi_mask: numpy.ndarray, *, keep_primary: bool = False, primary_min_area_px: int = 0) → numpy.ndarray

_darken_outside_roi(debug: numpy.ndarray, roi_mask: numpy.ndarray) → None

_lower_strip_color_counts(bgr: numpy.ndarray) → Tuple[int, int]

Return (color_a_pixels, color_b_pixels) in the bottom drive_out_strip_frac of the frame, cropped horizontally to the middle third so tape that only clips the corner of the FOV is ignored.

_middle_third_color_metrics(bgr: numpy.ndarray) → Tuple[int, float, str | None]

Look at the middle horizontal third of the frame (full height) and return (total_color_pixels, lateral_error_px, dominant_color):

• total_color_pixels — combined color_a+color_b pixel count in the crop

• lateral_error_px — centroid x of those pixels minus the crop’s center x; positive means colour is right of center

• dominant_color — "A", "B", or None if neither dominates / no colour is found

Returns (0, 0.0, None) if no colour is found.

_single_color_strip_metrics(bgr: numpy.ndarray, color: str) → Tuple[int, float]

Return (pixel_count, lateral_error_px) for a single colour ("A" or "B") within the bottom line_follow_strip_frac of the frame, full width. lateral_error_px = centroid x of that colour minus the strip’s center x. Positive = colour is right of center.

Returns (0, 0.0) if the colour is not found in the strip.

_update_drive_out(time_delta: float) → None

_turn_both_color_metrics(bgr: numpy.ndarray) → Tuple[int, float, str | None, numpy.ndarray, numpy.ndarray, int]

Both-colour metrics in the bottom-third crop (2h/3 to 9h/10, full width).

Same crop region as DLZNavigateMode’s corner logic, but masks both colours (matching the old TURN_TO_CENTER behaviour). Returns (total, lateral_error_px, dominant, mask_a, mask_b, row_start).

_centroid_align_step(*, count: int, lateral_px: float, min_pixels: int, center_tol_px: float, stable_count: int, stable_frames: int, search_angular: float, turn_angular_speed: float) → Tuple[str, int, float, bool]

Shared search/turn/lock controller for centroid-based alignment.

_update_turn_to_center(time_delta: float) → None

_update_line_follow(time_delta: float) → None

_black_centroid_metrics(bgr: numpy.ndarray) → Tuple[int, float]

Return (pixel_count, lateral_error_px) for black tape in the bottom line_follow_strip_frac of the frame, full width. lateral_error_px = centroid x minus strip center x; positive means tape is right of center.

_update_tape_align() → None

_DBG_COLOR_A = (0, 255, 255)

_DBG_COLOR_B = (0, 165, 255)

_DBG_CROP = (80, 80, 80)

_DBG_CENTER = (255, 255, 255)

_DBG_OK = (0, 255, 0)

_DBG_WARN = (0, 128, 255)

_DBG_TOL = (80, 160, 80)

_draw_shifted_contours(debug: numpy.ndarray, mask: numpy.ndarray, x_offset: int, y_offset: int, color: Tuple[int, int, int], thickness: int) → None

_put_label(debug: numpy.ndarray, text: str, y: int, color: Tuple[int, int, int] = (200, 200, 200), scale: float = 0.5, thickness: int = 1) → None

_publish_annotated(debug: numpy.ndarray) → None

_annotate_drive_out(bgr: numpy.ndarray, count_a: int, count_b: int) → None

_annotate_turn_to_center(bgr: numpy.ndarray, mask_a: numpy.ndarray, mask_b: numpy.ndarray, row_start: int, total: int, lateral_error_px: float, dominant: str | None, status: str) → None

_annotate_line_follow(bgr: numpy.ndarray, current: str, cur_count: int, cur_lateral_px: float, other: str, other_count: int, other_lateral_px: float, transitioned: bool, angular: float) → None

_annotate_tape_align(bgr: numpy.ndarray, count: int, lateral_px: float, status: str) → None