Work in progress: this documentation is still being written and reviewed. Check details against the source code.

Shared vision engine

Shared vision engine for camera acquisition, capture-time TF, multi-view scans, approach geometry, and configured collision objects. It also inherits MTC helpers from BaseStages. Some functions capture or move hardware; others only compute geometry. Its scan cache belongs to this engine instance and is not shared with other servers.

beambot.vision.vision_engine · Source

Perception and mapping · Guide · Symbol index

How it is used

Vision servers and sample stages own a VisionEngine to connect a camera backend with TF and motion support. Start with detect_and_transform_tag() or detect_and_transform_sample_roi() for base-frame results; compute_approach_pose() converts a detected pose into a tool approach.

scan_all_tags() and detect_tag_multiposition() also move the arm through scan poses. The tag cache belongs to the engine instance. Marker-linked collision objects are experimental and only added when configured; reset_tf() replaces transform resources after robot-model changes.

Related data types:

  • ObjectInfo: Configured collision primitive dimensions and marker-local placement offset.

  • GripperDetection: Resolved tool-tip frame and the configured vertical offset used for approach geometry.

Start here

Callable

Purpose

VisionEngine.detect_and_transform_tag

Try marker capture and base-frame conversion until one succeeds or the configured retry count is exhausted.

VisionEngine.detect_and_transform_sample_roi

Retry backend sample-ROI capture and transform the first successful pickup pose into base_link.

VisionEngine.compute_approach_pose

Choose an IK frame, rotate marker-local offsets into base_link, add a vertical offset, and construct a tool-down approach orientation.

VisionEngine.scan_all_tags

Visit scan poses, capture all visible markers repeatedly, and cache averages for markers seen at least twice.

VisionEngine.reset_tf

Replace cached transforms and restart the listener to receive latched transforms after model changes.

VisionEngine

BaseStages subclass combining camera acquisition, pose transforms, scan motion, approach geometry, and configured scene objects. Calls range from geometry calculations to hardware capture/motion, as identified in each method’s effects.

Inherits from: BaseStages.

VisionEngine.detect_and_transform_tag()

Try marker capture and base-frame conversion until one succeeds or the configured retry count is exhausted.

Method · Source

detect_and_transform_tag(self, tag_id: int, timeout: float=45.0) -> PoseStamped | None

Parameter

Meaning

tag_id

Requested marker ID.

timeout

Seconds given to each backend attempt; retries, settling, TF waits, and scene acknowledgement can add time.

Returns: First usable base_link PoseStamped or None after all attempts return unsuccessful results.

Errors: Ordinary empty/untransformable results are retried. Backend exceptions and collision-scene update exceptions propagate instead of being retried by a catch block.

Side effects: Triggers up to 1+retry_count captures, sleeps between retries, broadcasts detection TF, and may apply a configured collision object. Does not populate the scan cache.

Referenced in source (1)

VisionEngine.detect_and_transform_sample_roi()

Retry backend sample-ROI capture and transform the first successful pickup pose into base_link.

Method · Source

detect_and_transform_sample_roi(
    self,
    tag_id: int,
    strategy: str = 'farthest_edge',
    edge_inset_mm: float = 0.0,
    timeout: float = 45.0,
) -> PoseStamped | None

Parameter

Meaning

tag_id

Anchor marker ID.

strategy

Pickup selection strategy forwarded to the backend.

edge_inset_mm

Sample inset in millimetres.

timeout

Per-attempt backend timeout in seconds, not a total retry budget.

Returns: PoseStamped in base_link or None after unsuccessful attempts.

Errors: None results and failed transforms are retried. A backend lacking detect_sample_roi, such as the current ZED module, raises AttributeError; other backend exceptions are not caught.

Side effects: Triggers camera acquisition, sleeps between retries, and broadcasts sample_roi_tag<id>. It does not add a collision object or populate scan cache.

VisionEngine.compute_approach_pose()

Choose an IK frame, rotate marker-local offsets into base_link, add a vertical offset, and construct a tool-down approach orientation.

Method · Source

compute_approach_pose(
    self,
    target: PoseStamped,
    z_offset_override: float = 0.0,
    marker_offset_x: float = 0.0,
    marker_offset_y: float = 0.0,
    marker_offset_z: float = 0.0,
    ik_frame_override: str = '',
) -> tuple[PoseStamped, str]

Parameter

Meaning

target

PoseStamped assumed to already be in base_link. Its header is reused.

z_offset_override

Metres along base Z; zero selects the configured tool offset rather than requesting zero clearance.

marker_offset_x

Metres along marker-local X.

marker_offset_y

Metres along marker-local Y.

marker_offset_z

Metres along marker-local Z, before the separate base-Z offset.

ik_frame_override

Explicit IK frame. Otherwise a configured non-flange frame is used, then configured TF tip-frame detection.

Returns: (new approach PoseStamped, selected IK frame string). Orientation uses roll=180°, pitch=0°, and yaw derived after rotating marker orientation by 180° about Z.

Errors: No explicit input-frame, reachability, collision, or finite-value validation. Configuration, TF, and quaternion calculation errors can propagate.

Side effects: Reads configuration and may wait for tool-frame TF; logs geometry. Does not plan or execute motion.

VisionEngine.scan_all_tags()

Visit scan poses, capture all visible markers repeatedly, and cache averages for markers seen at least twice.

Method · Source

scan_all_tags(
    self,
    scan_positions: list[list[float]],
    scans_per_position: int = 3,
    timeout: float = 10.0,
    settle_time: float = 0.3,
) -> int

Parameter

Meaning

scan_positions

Sequence of six-joint poses in radians; _move_to_joint_pose checks each length.

scans_per_position

Number of captures per reached pose; nonpositive values produce no captures at that pose.

timeout

Seconds passed to each individual backend capture, not a deadline for the whole scan.

settle_time

Positive seconds to sleep after each successful move.

Returns: Number of marker IDs with averaged poses stored in _tag_pose_cache. Markers with only one usable detection are excluded.

Errors: Failed scan moves and unavailable transforms are skipped. Camera/averaging exceptions can propagate; no outer recovery or return-to-start move is provided.

Side effects: Clears the existing cache before scanning, executes PTP arm moves, captures images, waits for settling, and stores averaged base-frame poses. Does not publish the cache to another process.

VisionEngine.reset_tf()

Replace cached transforms and restart the listener to receive latched transforms after model changes.

Lifecycle · Source

reset_tf(self)

Returns: None.

Errors: TF/listener errors propagate.

Side effects: Unregisters the former listener, replaces _tf_buffer with a new Buffer, and creates a listener for it. It does not clear _tag_pose_cache.

VisionEngine.__init__()

Initialize shared motion support, camera selection, TF resources, scene client, collision-object configuration, and an empty scan cache.

Lifecycle · Source

__init__(
    self,
    rclpy_node,
    arm_group: str = '',
    ik_frame: str = '',
    camera_type: str = None,
    camera_frame: str = None,
    marker_dictionary: str = None,
    retry_count: int = None,
    retry_delay: float = None,
    settle_time: float = None,
)

Parameter

Meaning

rclpy_node

ROS node owning clients, TF resources, clock, and logger.

arm_group

MoveIt arm group forwarded to BaseStages; empty lets the base configuration choose it.

ik_frame

Configured IK tip frame; empty/flange permits later tool-frame detection for approach geometry.

camera_type

Required truthy backend key, currently zivid or zed.

camera_frame

Required camera TF frame used to transform detections into base_link.

marker_dictionary

Required marker dictionary name sent to the backend.

retry_count

Retries after the first detection attempt; None selects 3.

retry_delay

Seconds between retry attempts; None selects 0.5.

settle_time

Pre-capture settling seconds passed to compatible backends; None selects 0.3.

Returns: None; initializes an engine instance.

Errors: After base initialization, missing camera fields raise ValueError. Backend imports/client/TF construction errors propagate. Retry/time values are not range-validated.

Side effects: Initializes BaseStages, creates TF listener/broadcaster and ROS clients, loads cached beamline configuration, and logs. Does not itself request detection or dispatch a scan move.

Referenced in source (4)

VisionEngine.get_cached_pose()

Return a pose from this engine’s scan cache.

Method · Source

get_cached_pose(self, tag_id: int) -> PoseStamped | None

Parameter

Meaning

tag_id

Marker ID used as the dictionary key.

Returns: Stored PoseStamped object or None; it is returned without copying or freshness checks.

Side effects: Reads _tag_pose_cache without triggering detection or motion.

VisionEngine.detect_tag_multiposition()

Visit supplied scan poses and average at least two successful detections of one marker.

Method · Source

detect_tag_multiposition(
    self,
    tag_id: int,
    scan_positions: list[list[float]],
    timeout: float = 45.0,
    settle_time: float = 0.3,
) -> PoseStamped | None

Parameter

Meaning

tag_id

Marker ID to detect.

scan_positions

Joint-pose sequence in radians.

timeout

Seconds per detection attempt; each position can also incur engine retries and retry delay.

settle_time

Positive settling sleep after reaching each scan pose, in seconds.

Returns: Averaged base_link PoseStamped or None if fewer than two detections succeed. This method does not insert the average into _tag_pose_cache.

Errors: Move/detection failures are logged and skipped. Exceptions from delegated detection, scene updates, or averaging can propagate.

Side effects: Moves the arm, captures detections, logs spread, and can broadcast detection TF/update configured collision objects through detect_and_transform_tag(). Leaves the arm at the final reached pose.

Internal callbacks and helpers

VisionEngine._load_vision_objects_config()

Read marker-keyed collision-object definitions from beamline configuration.

Internal helper · Source

_load_vision_objects_config(self)

Returns: None; valid entries populate _object_database.

Errors: Configuration-load exceptions are logged and return. Entries with missing keys, invalid tag IDs, or type/value errors are logged and skipped. A non-mapping collision_objects container can still raise outside the per-entry handler.

Side effects: Reads process-cached YAML and stores ObjectInfo records. Existing entries are not cleared on repeated calls; geometry dimensions are not fully validated here.

Referenced in source (1)

VisionEngine._move_to_joint_pose()

Build and execute a PTP scan move from six radian-valued joint angles.

Internal helper · Source

_move_to_joint_pose(self, joint_positions: list[float]) -> bool

Parameter

Meaning

joint_positions

Exactly six joint angles in radians, zipped against DEFAULT_JOINT_NAMES.

Returns: True if load_plan_execute() reports no error; otherwise False.

Errors: Wrong length returns False with a log. A returned planning/execution error also becomes False. Stage/model/planner exceptions are not caught here.

Side effects: Creates an MTC task and dispatches real arm trajectory execution through shared motion helpers.

Referenced in source (2)

VisionEngine._average_poses()

Average positions arithmetically and average quaternions after aligning signs to the first quaternion.

Internal helper · Source

_average_poses(self, poses: list[PoseStamped]) -> PoseStamped | None

Parameter

Meaning

poses

At least two PoseStamped values already expressed in a common frame.

Returns: New PoseStamped using the first input header and a normalized mean quaternion; None for fewer than two inputs.

Errors: No frame consistency, finite-value, or zero-quaternion-norm validation. Degenerate quaternions can produce nonfinite values; malformed inputs can raise.

Side effects: Allocates a pose and numeric arrays; no capture, TF request, or motion.

Referenced in source (2)

VisionEngine._single_detection_attempt()

Request one marker from the configured backend using engine camera and settling settings.

Internal helper · Source

_single_detection_attempt(self, tag_id: int, timeout: float) -> DetectionResult | None

Parameter

Meaning

tag_id

Single requested marker ID.

timeout

Per-backend capture budget in seconds.

Returns: DetectionResult if its markers list is nonempty, otherwise None. Exact marker matching is done later.

Errors: Backend exceptions propagate. Empty marker results are logged and converted to None.

Side effects: Triggers camera acquisition through the backend and logs; no motion is added here.

Referenced in source (1)

VisionEngine._process_detection_result()

Find the requested marker, transform it to base_link, and update its detection frame and configured collision object.

Internal helper · Source

_process_detection_result(
    self,
    tag_id: int,
    detection_result: DetectionResult,
) -> PoseStamped | None

Parameter

Meaning

tag_id

Marker ID to select from detection_result.markers.

detection_result

DetectionResult containing camera-frame poses and an optional capture timestamp.

Returns: Transformed PoseStamped, or None when the marker is absent or the transform is unavailable.

Errors: Invalid configured collision geometry or failed planning-scene acknowledgement can raise after TF has been broadcast. Other message errors are not caught.

Side effects: Broadcasts aruco_<id> and may call /apply_planning_scene. Does not write the averaged scan cache.

Referenced in source (1)

VisionEngine._broadcast_detection_tf()

Publish a named detection frame relative to base_link.

Internal helper · Source

_broadcast_detection_tf(self, frame_name: str, pose: PoseStamped)

Parameter

Meaning

frame_name

Child TF frame ID to broadcast.

pose

PoseStamped whose numeric pose is assumed to already be in base_link; its header frame is not validated.

Returns: None.

Errors: Message/TF publication errors propagate.

Side effects: Broadcasts one transform stamped with the current node time, rather than the original capture time.

Referenced in source (2)

VisionEngine._apply_flange_offset()

Shift an existing pose along the current flange axes after rotating the selected direction into base_link.

Internal helper · Source

_apply_flange_offset(
    self,
    pose: PoseStamped,
    direction: str,
    distance: float,
) -> PoseStamped

Parameter

Meaning

pose

PoseStamped in base_link, modified in place.

direction

DIRECTION_VECTORS alias. forward/x=+X, backward/-x=-X, right/y=-Y, left/-y=+Y, up/z=+Z, down/-z=-Z in flange coordinates.

distance

Displacement in metres; this helper itself does not enforce positivity or finiteness.

Returns: The same PoseStamped object after its position changes.

Errors: Unknown direction raises ValueError. TransformException becomes RuntimeError. Other numeric errors propagate.

Side effects: Queries latest base_link←flange TF with a two-second timeout, mutates the supplied pose, and logs the offset.

VisionEngine._move_to_approach()

Resolve deterministic IK and execute a constrained PTP joint goal, or a constrained LIN pose goal when IK has no solution.

Internal helper · Source

_move_to_approach(
    self,
    approach: PoseStamped,
    ik_frame: str = '',
    constraints = None,
) -> 'str | None'

Parameter

Meaning

approach

Target PoseStamped, normally in base_link.

ik_frame

IK tip frame; empty invokes configured-frame detection.

constraints

Optional MoveIt Constraints object applied to the motion stage; None leaves path constraints unset.

Returns: None on success, or an error string from load_plan_execute().

Errors: No catch for model/stage/IK errors. An unavailable deterministic joint goal changes the planning path to LIN; no further fallback chain is added here.

Side effects: Builds and executes an MTC motion task with any supplied path constraints.

VisionEngine._detect_current_gripper()

Find the first configured tip frame visible in TF and obtain its vertical offset.

Internal helper · Source

_detect_current_gripper(self) -> GripperDetection

Returns: GripperDetection(frame, z_offset), falling back to (flange, 0.0). This is frame availability, not a physical attachment sensor.

Errors: No local exception handler. Each can_transform check may wait up to one second.

Side effects: Reads configured tip frames, queries TF in order, and logs the fallback.

Referenced in source (2)

VisionEngine._z_offset_for_frame()

Look up the beamline’s vertical offset for a tip frame.

Internal helper · Source

@staticmethod
_z_offset_for_frame(ik_frame: str) -> float

Parameter

Meaning

ik_frame

Configured tip frame name.

Returns: Configured float offset in metres, with helper default 0.0 when unavailable.

Errors: Delegates fallback/error handling to config_loader.z_offset_for_tip_frame().

Side effects: Reads shared beamline configuration through the helper; no model relaunch.

Referenced in source (2)

VisionEngine._add_collision_object_for_tag()

Add the configured collision primitive at a detected marker pose. Marker-linked collision objects are experimental.

Internal helper · Source

_add_collision_object_for_tag(self, tag_id: int, tag_pose: PoseStamped)

Parameter

Meaning

tag_id

Marker ID used to look up _object_database.

tag_pose

Detected PoseStamped used as the marker frame for the object offset.

Returns: None; an unconfigured tag returns immediately.

Errors: ValueError for unsupported shape/dimension count. Only box with three dimensions and cylinder with two are accepted; service acknowledgement failures propagate.

Side effects: Calculates object placement, creates a CollisionObject.ADD message, calls /apply_planning_scene, and logs success after acknowledgement. Cylinder dimensions follow SolidPrimitive order (height, radius).

Referenced in source (1)

VisionEngine._calculate_object_pose()

Rotate a marker-local offset into the marker pose’s frame while preserving its orientation.

Internal helper · Source

_calculate_object_pose(self, tag_pose: PoseStamped, offset: list) -> PoseStamped

Parameter

Meaning

tag_pose

PoseStamped with the marker position/orientation.

offset

Three-element marker-local XYZ displacement in metres.

Returns: New PoseStamped with translated position; header and orientation are assigned from the input.

Errors: No dimension, frame, or finite-value validation; array/quaternion errors propagate.

Side effects: Allocates a result and numeric arrays; does not query TF or publish the object.

Referenced in source (1)

VisionEngine._apply_collision_object()

Submit one world collision-object change and require a successful MoveGroup response.

Internal helper · Source

_apply_collision_object(self, obj: CollisionObject) -> None

Parameter

Meaning

obj

CollisionObject to append to a PlanningScene diff.

Returns: None after acknowledgement.

Errors: RuntimeError for missing service, a two-second future timeout, absent response, or success=false. Other service/future errors propagate. A timed-out future is not canceled here.

Side effects: Calls /apply_planning_scene and waits; can change the planning environment even if later task execution fails.

Referenced in source (1)