payload.modes.PayloadDLZNavigateMode#

Attributes#

`_CORNER_CENTER_TOL_PX`
`_CORNER_CENTER_MIN_PX`
`_CORNER_STABLE_FRAMES`
`_CORNER_MAX_RAD`
`_CORNER_PRE_TURN_WAIT_S`
`_TURN_CENTER_TOL_PX`
`_TURN_CENTER_MIN_PX`
`_TURN_STABLE_FRAMES`
`_TURN_MAX_RAD`
`_VALID_START_PHASES`
`_STRIP_START_FRAC`
`_MIN_COLOR_PIXELS`
`_COLOR_RATIO`
`_MIN_ROW_PIXELS`
`_COLOR_BLUR_KERNEL`

Classes#

`TagTransitionRule`	dict() -> new empty dictionary
`PayloadDLZNavigateParams`
`PayloadDLZNavigateMode`	Navigate the payload along the alternating-colour square border of the DLZ.

Functions#

`_get_strip`(→ Tuple[numpy.ndarray, int])	Return the bottom-4/9 strip and its y-offset in the full frame.
`_preprocess_color_crop`(→ numpy.ndarray)	Light denoising before HSV thresholding to stabilize tape masks.
`_detect_current_color`(→ str)	Determine dominant tape color. Returns "A" (red), "B" (blue), or "none".
`_detect_tape_following`(→ Tuple[bool, float, float])	Find tape centre and return (boundary_detected, lateral_error_px, boundary_angle).

Module Contents#

_CORNER_CENTER_TOL_PX = 75.0#

_CORNER_CENTER_MIN_PX = 400#

_CORNER_STABLE_FRAMES = 2#

_CORNER_MAX_RAD = 6.283185307179586#

_CORNER_PRE_TURN_WAIT_S = 0.0#

_TURN_CENTER_TOL_PX = 50.0#

_TURN_CENTER_MIN_PX = 150#

_TURN_STABLE_FRAMES = 2#

_TURN_MAX_RAD = 6.283185307179586#

_VALID_START_PHASES = ('wait_for_plane', 'scan_tags', 'line_follow')#

_STRIP_START_FRAC = 0.6666666666666666#

_MIN_COLOR_PIXELS = 20#

_COLOR_RATIO = 1.5#

_MIN_ROW_PIXELS = 3#

_COLOR_BLUR_KERNEL = (5, 5)#

_get_strip(bgr: numpy.ndarray) → Tuple[numpy.ndarray, int]#: Return the bottom-4/9 strip and its y-offset in the full frame.

_preprocess_color_crop(bgr: numpy.ndarray) → numpy.ndarray#: Light denoising before HSV thresholding to stabilize tape masks.

_detect_current_color(orange_mask: numpy.ndarray, blue_mask: numpy.ndarray) → str#: Determine dominant tape color. Returns “A” (red), “B” (blue), or “none”.

_detect_tape_following(orange_mask: numpy.ndarray, blue_mask: numpy.ndarray) → Tuple[bool, float, float]#: Find tape centre and return (boundary_detected, lateral_error_px, boundary_angle).

class TagTransitionRule#

Bases: TypedDict

dict() -> new empty dictionary dict(mapping) -> new dictionary initialized from a mapping object’s

(key, value) pairs

dict(iterable) -> new dictionary initialized as if via:: d = {} for k, v in iterable:

d[k] = v
dict(**kwargs) -> new dictionary initialized with the name=value pairs: in the keyword argument list. For example: dict(one=1, two=2)

transitions: int#

direction: Literal['cw', 'ccw']#

class PayloadDLZNavigateParams#

Bases: vehicle_common.mode_loader.ParamsBase

direction: Literal['cw', 'ccw'] = 'ccw'#

target_transitions: int = 1#

turn_angular_speed: float = 0.5#

corner_angular_speed: float = 0.3#

line_follow_speed_mps: float = 0.1#

k_lat: float = 0.003#

k_d_lat: float = 0.002#

k_ang: float = 0.4#

max_angular: float = 0.5#

tag_transition_table: dict[str, TagTransitionRule] | None = None#

detect_frames: int = 5#

scan_duration_s: float = 1.0#

start_phase: Literal['wait_for_plane', 'scan_tags', 'line_follow'] = 'wait_for_plane'#

tag_size_m: float = 0.0508#

tag_family: str = 'tag36h11'#

compressed_image: bool = False#

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

cw_lower_hsv2: list[int] = (255, 255, 255)#

cw_upper_hsv2: list[int] = (255, 255, 255)#

class PayloadDLZNavigateMode#

Bases: vehicle_common.mode.Mode

Navigate the payload along the alternating-colour square border of the DLZ.

Phase sequence (controlled by start_phase):

WAIT_FOR_PLANE → SCAN_TAGS → TURN_ONTO_TAPE → LINE_FOLLOW

WAIT_FOR_PLANE

Hold still facing inward. When any AprilTag is visible for detect_frames consecutive frames the plane is confirmed landed → advance to SCAN_TAGS.

SCAN_TAGS

Hold still for scan_duration_s and accumulate all visible tag IDs. Look up the tag combo in tag_transition_table to determine direction and target_transitions. If no match, fall back to the constructor defaults. If target_transitions == 0 → done immediately (already at the dock side).

TURN_ONTO_TAPE

Rotate in place until the combined red/blue tape is centred in the detection strip (vision-based; safety timeout at 2π rad):

direction=”cw” → turn left (positive angular) direction=”ccw” → turn right (negative angular)

LINE_FOLLOW

Follow the combined red/blue tape strip. Detect colour transitions (A↔B) to count segments and execute corner turns:

direction=”cw” : B→A transition = corner → turn right 90° direction=”ccw” : A→B transition = corner → turn left 90°

Stop after target_transitions total colour changes.

tag_transition_table key format

Keys use ‘,’ to separate AND-groups and ‘|’ for OR within a group. “0|4|5,2” = (tag 0 OR 4 OR 5 visible) AND (tag 2 visible). Test keys in YAML order — put more-specific rules first.

required_vision_nodes#

transition_labels = ('done',)#

requires_camera = True#

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

_turn_angular() → float#: Signed angular speed for the initial tape-alignment turn. CW direction → turn left (positive). CCW direction → turn right (negative).

_corner_transition(prev: str, curr: str) → bool#: True when the A↔B transition is a corner for the current travel direction.

_request_apriltag_state() → uav_interfaces.srv.PayloadAprilTagState.Response | None#

_image_cb(msg) → None#

_decode_image() → numpy.ndarray | None#

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

_threshold_color_mask(hsv: numpy.ndarray, roi_mask: numpy.ndarray, *, color: str) → numpy.ndarray#

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

_detect_color(bgr: numpy.ndarray) → Tuple[str, bool, float, float, numpy.ndarray, numpy.ndarray, numpy.ndarray, int]#

Run the full colour detection pipeline on a BGR frame.

Returns (current_color, boundary_detected, lateral_error_px,: boundary_angle, orange_mask, blue_mask, strip, strip_start).

_publish_annotated(debug: numpy.ndarray) → None#

_match_table(seen_ids: set[int]) → dict | None#: Match seen tag IDs against tag_transition_table. Key format: comma-separated AND-groups, each group pipe-separated ORs. Returns the first matching entry dict, or None.

on_enter() → None#

on_update(time_delta: float) → None#

check_status() → str#

on_exit() → None#

_update_wait_for_plane() → None#

_update_scan_tags(time_delta: float) → None#

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

Middle-third × bottom-40% crop, masked on a single expected colour.

Returns (pixel_count, lateral_error_px, mask, row_start, col_start). lateral_error_px is the mask centroid x minus crop center x (positive = right). Only the expected colour is computed — the opposite-side tape cannot bias the centroid.

_update_turn_onto_tape(time_delta: float) → None#

_annotate_turn_onto_tape(bgr: numpy.ndarray, row_start: int, col_start: int, mask: numpy.ndarray, expected_color: str, count: int, lateral_error_px: float, angular: float) → None#

_update_line_follow(time_delta: float) → None#

_corner_single_color_metrics(bgr: numpy.ndarray, color: str) → Tuple[int, float, numpy.ndarray, int, int]#: Return (pixel_count, lateral_error_px, mask, row_start, col_start) for a single colour in the bottom third of the frame, full frame width. Returns (0, 0.0, empty_mask, row_start, col_start) if no pixels found.

_do_corner_turn(time_delta: float) → None#

_annotate_corner_turn(bgr: numpy.ndarray, mask: numpy.ndarray, row_start: int, col_start: int, total: int, lateral_error_px: float) → None#

_annotate_line_follow(bgr: numpy.ndarray, strip_start: int, orange_mask: numpy.ndarray, blue_mask: numpy.ndarray, current_color: str, boundary_detected: bool, lateral_error_px: float, boundary_angle: float, angular: float, transitioned: bool, is_corner: bool) → None#