Follow a fiducial tagΒΆ
The warehouse forklift has a 0.40 m DICT_4X4_50 marker (ID 0) on its rear body face. The example uses a forward-facing camera and camera info to solve the tag pose, then sends short velocity leases to keep the robot base 2 m behind the tag plane.
Start warehouse traffic in the simulator viewer after placing the robot behind the forklift with the tag in view.
Note
This works best with a fast omnidirectional robot: the follower strafes toward the standoff point while turning to face the tag. This is a simple visual servo example with no mapping or obstacle avoidance. The forklift can reach 0.5 m/s, so slower robots may fall behind.
import math
from olo import Client
from olo.errors import OloFailedPrecondition, OloNotFound, OloTimeoutError
from olo.spatial import Point
RGB_TOPIC = "camera"
CAMERA_INFO_TOPIC = "camera/camera_info"
TAG_FAMILY = "aruco_4x4_50"
TAG_ID = 0
MARKER_SIZE_M = 0.40 # Black outer edge; the white mounting margin is excluded.
STANDOFF_M = 2.0 # From the tag plane to the robot base origin.
VELOCITY_LEASE_S = 0.5
POSITION_GAIN = 0.5
LATERAL_GAIN = 1.0
HEADING_GAIN = 1.0
POSITION_DEADBAND_M = 0.25
HEADING_DEADBAND_RAD = 0.15
with Client() as client:
robot = client.robot()
rgb_topic = robot.core.topic(RGB_TOPIC, "sensor_msgs/msg/Image")
info_topic = robot.core.topic(CAMERA_INFO_TOPIC, "sensor_msgs/msg/CameraInfo")
base_frame = robot.frames.base_frame() or "base_link"
detector = robot.vision.tags(TAG_FAMILY, size_m=MARKER_SIZE_M)
print(f"Following tag {TAG_ID}; waiting for camera frames")
with detector.subscribe(rgb_topic, info_topic) as stream:
tag_visible = False
logged_command = False
for index, frame in enumerate(stream):
tag = next((tag for tag in frame.tags if tag.id == TAG_ID), None)
if tag is None or tag.transform is None:
if tag_visible or index % 10 == 0:
print(f"Tag {TAG_ID} lost/not found; no command (velocity lease expires)")
tag_visible = False
logged_command = False
continue
newly_found = not tag_visible
tag_visible = True
# Offset along the tag's +Z axis, toward the observing camera.
# Re-express that offset in the robot base frame at image time.
try:
tag_in_base = tag.to_frame(robot.frames, base_frame, timeout=0.2)
except (OloNotFound, OloTimeoutError):
if newly_found or index % 10 == 0:
camera_position = tag.transform.translation
print(
f"Tag {TAG_ID} found at x={camera_position.x:.2f} "
f"y={camera_position.y:.2f} z={camera_position.z:.2f} m "
"in camera frame; base transform unavailable, no command"
)
continue
position = tag_in_base.translation
target = tag_in_base.apply(Point(z=STANDOFF_M))
tag_position = (
f"Tag {TAG_ID} found at x={position.x:.2f} y={position.y:.2f} "
f"z={position.z:.2f} m in {base_frame}"
)
# Face the tag while translating toward the offset standoff point.
bearing = math.atan2(position.y, max(0.3, abs(position.x)))
position_error = math.hypot(target.x, target.y)
yaw = HEADING_GAIN * bearing if abs(bearing) >= HEADING_DEADBAND_RAD else 0.0
forward = (
POSITION_GAIN * target.x
if position_error >= POSITION_DEADBAND_M
else 0.0
)
lateral = LATERAL_GAIN * target.y if position_error >= POSITION_DEADBAND_M else 0.0
if forward == lateral == yaw == 0.0:
if newly_found or index % 10 == 0:
print(f"{tag_position}; at {STANDOFF_M:.1f} m standoff, no motion command")
continue
try:
applied = robot.locomotion.send_velocity(
x=forward, y=lateral, yaw=yaw, lease=VELOCITY_LEASE_S
)
except OloFailedPrecondition as error:
if "base stop already in progress" not in str(error):
raise
if newly_found or index % 10 == 0:
print(f"{tag_position}; command deferred while previous base stop finishes")
continue
if not logged_command or index % 10 == 0:
print(
f"{tag_position}; standoff target x={target.x:.2f} y={target.y:.2f} m; "
f"commanding x={forward:.2f} y={lateral:.2f} m/s yaw={yaw:.2f} rad/s; "
f"applied x={applied.applied.x:.2f} y={applied.applied.y:.2f} "
f"yaw={applied.applied.yaw:.2f} "
f"on {applied.cmd_vel_topic}"
)
logged_command = True
import { OloFailedPrecondition, OloNotFound, OloTimeoutError } from "olo";
import { connect } from "olo/web";
import { Point } from "olo/spatial";
import { tagToFrame } from "olo/vision";
const RGB_TOPIC = "camera";
const CAMERA_INFO_TOPIC = "camera/camera_info";
const TAG_FAMILY = "aruco_4x4_50";
const TAG_ID = 0;
const MARKER_SIZE_M = 0.4; // Black outer edge, excluding the white margin.
const STANDOFF_M = 2.0; // From the tag plane to the robot base origin.
const VELOCITY_LEASE_MS = 500;
const POSITION_GAIN = 0.5;
const LATERAL_GAIN = 1.0;
const HEADING_GAIN = 1.0;
const POSITION_DEADBAND_M = 0.25;
const HEADING_DEADBAND_RAD = 0.15;
const robot = await connect().robot();
const rgbTopic = robot.core.topic(RGB_TOPIC, "sensor_msgs/msg/Image");
const cameraInfoTopic = robot.core.topic(
CAMERA_INFO_TOPIC,
"sensor_msgs/msg/CameraInfo",
);
const baseFrame = (await robot.frames.baseFrame()) ?? "base_link";
const detector = robot.vision.tags(TAG_FAMILY, { sizeM: MARKER_SIZE_M });
const stream = detector.subscribe(rgbTopic, { cameraInfoTopic });
console.log(`Following tag ${TAG_ID}; waiting for camera frames`);
try {
let index = 0;
let tagVisible = false;
let loggedCommand = false;
for await (const frame of stream) {
const logFrame = index++ % 10 === 0;
const tag = frame.tags.find(
(candidate) =>
candidate.id === TAG_ID && candidate.transform !== undefined,
);
if (!tag?.transform) {
if (tagVisible || logFrame)
console.log(
`Tag ${TAG_ID} lost/not found; no command (velocity lease expires)`,
);
tagVisible = false;
loggedCommand = false;
continue;
}
const newlyFound = !tagVisible;
tagVisible = true;
// Offset along the tag's +Z axis, toward the observing camera.
let target: Point;
let tagPoint: Point;
let tagPosition: string;
try {
const tagInBase = await tagToFrame(tag, {
frames: robot.frames,
targetFrame: baseFrame,
timeoutMs: 200,
});
const position = tagInBase.translation;
tagPoint = position;
tagPosition = `Tag ${TAG_ID} found at x=${position.x.toFixed(2)} y=${position.y.toFixed(2)} z=${position.z.toFixed(2)} m in ${baseFrame}`;
target = tagInBase.apply(new Point(0, 0, STANDOFF_M));
} catch (error) {
if (!(error instanceof OloNotFound || error instanceof OloTimeoutError))
throw error;
if (newlyFound || logFrame) {
const position = tag.transform.translation;
console.log(
`Tag ${TAG_ID} found at x=${position.x.toFixed(2)} y=${position.y.toFixed(2)} ` +
`z=${position.z.toFixed(2)} m in camera frame; base transform unavailable, no command`,
);
}
continue;
}
// Face the tag while translating toward the offset standoff point.
const bearing = Math.atan2(tagPoint.y, Math.max(0.3, Math.abs(tagPoint.x)));
const positionError = Math.hypot(target.x, target.y);
const yaw =
Math.abs(bearing) >= HEADING_DEADBAND_RAD ? HEADING_GAIN * bearing : 0;
const forward =
positionError >= POSITION_DEADBAND_M ? POSITION_GAIN * target.x : 0;
const lateral =
positionError >= POSITION_DEADBAND_M ? LATERAL_GAIN * target.y : 0;
if (forward === 0 && lateral === 0 && yaw === 0) {
if (newlyFound || logFrame)
console.log(
`${tagPosition}; at ${STANDOFF_M.toFixed(1)} m standoff, no motion command`,
);
continue;
}
try {
const applied = await robot.locomotion.sendVelocity({
x: forward,
y: lateral,
yaw,
leaseMs: VELOCITY_LEASE_MS,
});
if (!loggedCommand || logFrame) {
console.log(
`${tagPosition}; standoff target x=${target.x.toFixed(2)} y=${target.y.toFixed(2)} m; ` +
`commanding x=${forward.toFixed(2)} y=${lateral.toFixed(2)} m/s yaw=${yaw.toFixed(2)} rad/s; ` +
`applied x=${applied.applied.x.toFixed(2)} y=${applied.applied.y.toFixed(2)} yaw=${applied.applied.yaw.toFixed(2)} ` +
`on ${applied.cmdVelTopic}`,
);
loggedCommand = true;
}
} catch (error) {
if (
!(
error instanceof OloFailedPrecondition &&
String(error).includes("base stop already in progress")
)
) {
throw error;
}
if (newlyFound || logFrame)
console.log(
`${tagPosition}; command deferred while previous base stop finishes`,
);
}
}
} finally {
stream.close();
}