You can use the following tool to display the camera feeds in opencv, rerun and store them locally anywhere from the command line: stretch_camera_show.
Run stretch_camera_show --help for all the available options.
Examples:
stretch_camera_show --left --opencv # Display the left head camera only
stretch_camera_show --right --opencv # Display the right head camera only
stretch_camera_show --center --opencv # Display the center head camera only
stretch_camera_show --left_right --opencv # Display the left, right head cameras only
stretch_camera_show --left_right_center --opencv # Display the left, right and center head cameras
stretch_camera_show --gripper --rerun # Display the gripper camera feeds and the point cloud from the stereo depth
stretch_camera_show --left --recording_directory ./recordings # Store images to disk
The camera subsystem exposes Python generators for streaming frames from the cameras. These generators yield ImageFrame or SyncedImageFrame objects.
ImageFrame is a container for a single camera's image and metadata. Access the image using the image attribute, timestamp with timestamp, and AI model results with ai_model_results.
SyncedImageFrame is a container for multiple cameras' images and metadata. You can access the individual camera frames using the left, right, and center attributes of the SyncedImageFrame object, which are ImageFrame objects.
Using one camera yields an ImageFrame.
from stretch4_body.subsystem.cameras import *
for image_frame in stream_left_camera():
if image_frame is None:
print("No frame returned")
continue
print(f"Got image: {image_frame.image.shape=}, {image_frame.timestamp=}")Using multiple cameras from the head cameras simultaneously yields a SyncedImageFrame.
from stretch4_body.subsystem.cameras import *
for image_frame in stream_left_right_camera():
if image_frame is None:
print("No frame returned")
continue
print(f"Got left image: {image_frame.left.image.shape=}, {image_frame.left.timestamp=}")
print(f"Got right image: {image_frame.right.image.shape=}, {image_frame.right.timestamp=}")Stream from the gripper RGBD camera. This yields a SyncedImageFrame and populates the pointcloud field.
from stretch4_body.subsystem.cameras import *
for image_frame in stream_gripper_camera():
if image_frame is None:
print("No frame returned")
continue
print(f"Got left image: {image_frame.left.image.shape=}, {image_frame.left.timestamp=}")
if image_frame.pointcloud is not None:
print(f"Got pointcloud image: {image_frame.pointcloud.shape=}")Run stretch_rgbd_show --help for all the available options.
Examples:
stretch_rgbd_show --left_right # Display the left, right head cameras only
stretch_rgbd_show --left_right_center # Display the left, right and center head cameras
stretch_rgbd_show --left # Display the left head camera only
stretch_rgbd_show --right # Display the right head camera only
stretch_rgbd_show --center # Display the center head camera only
If you have ROS2 launch files running for cameras or lidars, you can use the --use_ros_for_cameras or --use_ros_for_lidar flags to use the ROS2 data instead of direct python-api access to those sensors.
stretch_rgbd_show --left --use_ros_for_cameras --use_ros_for_lidar
You can capture RGBD data from the head cameras using the following scripts:
from stretch4_body.subsystem.cameras import *
for frame in stream_left_rgbd():
if frame is None:
print("No frame returned")
continue
print(f"""Got a point cloud using the left camera and both lidar.
Number of points: {frame.pointcloud.shape[0]}
Depth size: {frame.depth_image.shape}""")
for frame in stream_left_right_center_rgbd():
if frame is None:
print("No frame returned")
continue
print(f"""Got a point cloud using the left, right and center cameras and both lidar.
Left:
Number of points: {frame.left.pointcloud.shape[0]}
Depth size: {frame.left.depth_image.shape}
Right:
Number of points: {frame.right.pointcloud.shape[0]}
Depth size: {frame.right.depth_image.shape}
Center:
Number of points: {frame.center.pointcloud.shape[0] if frame.center is not None else 'N/A'}
Depth size: {frame.center.depth_image.shape if frame.center is not None else 'N/A'}""")You can pass custom AI models to the camera pipeline by wrapping them in an AIModelWrapper instance. The pipeline will automatically route imagery to your model. The results are packed in the image_frame.ai_model_results field.
Here is an example wrapping an RTMO model:
First install
git clone https://github.com/hello-robot/stretch4_human_pose_estimation.git
pip install ./stretch4_human_pose_estimation
human_pose_estimation_install_dependencies
human_pose_estimation_setup_models
human_pose_estimation_setup_models --size m
Then run this example:
import cv2
import numpy as np
from stretch4_body.subsystem.cameras import stream_left_camera
from stretch4_body.subsystem.cameras.detectors.detector_ai_models import AIModelWrapper
from stretch4_human_pose_estimation.rtmo import RTMOPipeline
class RTMOWrapper(AIModelWrapper):
def __init__(self):
self.model = self.init_model()
def name(self) -> str:
return "RTMO Pose"
def init_model(self):
return RTMOPipeline(size="m", device="AUTO")
def run_model(self, img: np.ndarray, conf_threshold: float = 0.5):
return self.model.predict(img, conf_threshold=conf_threshold)
def visualize_results(self, img: np.ndarray, result_from_run_model) -> np.ndarray:
return self.model.visualize(img, result_from_run_model, kpt_thr=0.3, style="cvpr")
@staticmethod
def get_joint(person_res: dict, joint: BodyJoint) -> tuple[float, float]:
"""Returns the (x, y) coordinates of the specified joint for a single person."""
kpts = person_res.get("keypoints", [])
if joint < len(kpts):
return float(kpts[joint][0]), float(kpts[joint][1])
return 0.0, 0.0
@staticmethod
def get_joint_score(person_res: dict, joint: BodyJoint) -> float:
"""Returns the confidence score of the specified joint for a single person."""
kpts = person_res.get("keypoints", [])
if joint < len(kpts):
return float(kpts[joint][2])
return 0.0
# Instantiate the custom model wrapper
rtmo = RTMOWrapper()
# Start the left camera stream passing the AI model to the pipeline
for image_frame in stream_left_camera(ai_models_to_use=[rtmo]):
if image_frame is None:
continue
results = image_frame.ai_model_results[0]
annotated_image = rtmo.visualize_results(image_frame.image.copy(), results)
cv2.namedWindow(rtmo.name(), cv2.WINDOW_NORMAL)
cv2.imshow(rtmo.name(), annotated_image)
if cv2.waitKey(1) == ord('q'):
breakHere is another example wrapping a YOLOX object detection model (from rtmlib) configured to identify general COCO objects like cups, tables, desks, and chairs.
First install rtmlib:
pip install rtmlib
pip install openvino # To utilize the NPU on Stretch's NUC
pip install onnxruntime-openvino
The model weights will be automatically downloaded by rtmlib on the first run.
import cv2
import numpy as np
from stretch4_body.subsystem.cameras import *
from stretch4_body.subsystem.cameras.detectors.detector_ai_models import AIModelWrapper
# RTMLIB imports
from rtmlib import YOLOX
from rtmlib.tools.base import RTMLIB_SETTINGS
# Configure rtmlib to utilize the Intel GPU or NPU via OpenVINO Execution Provider
RTMLIB_SETTINGS['onnxruntime']['npu'] = ('OpenVINOExecutionProvider', {'device_type': 'NPU'})
RTMLIB_SETTINGS['onnxruntime']['gpu'] = ('OpenVINOExecutionProvider', {'device_type': 'GPU'})
class YOLOXWrapper(AIModelWrapper):
def __init__(self):
self.coco_classes = [
"person", "bicycle", "car", "motorcycle", "airplane", "bus", "train", "truck", "boat", "traffic light",
"fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", "horse", "sheep", "cow",
"elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", "tie", "suitcase", "frisbee",
"skis", "snowboard", "sports ball", "kite", "baseball bat", "baseball glove", "skateboard", "surfboard",
"tennis racket", "bottle", "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana", "apple",
"sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", "chair", "couch",
"potted plant", "bed", "dining table", "toilet", "tv", "laptop", "mouse", "remote", "keyboard",
"cell phone", "microwave", "oven", "toaster", "sink", "refrigerator", "book", "clock", "vase",
"scissors", "teddy bear", "hair drier", "toothbrush", "monitor", "can", "bottle", "tennis ball", "ball", "tape", "mug", "remote control", "desk"
]
self.model = self.init_model()
def name(self) -> str:
return "YOLOX Object Detection"
def init_model(self):
device = 'gpu' # npu, cpu, gpu
backend = 'onnxruntime' # We use onnxruntime to route to the OpenVINO NPU Provider
# Provide a URL to a COCO yolox model. rtmlib will cache it automatically.
url = 'https://github.com/Megvii-BaseDetection/YOLOX/releases/download/0.1.1rc0/yolox_tiny.onnx'
return YOLOX(url, mode='multiclass',model_input_size=(416, 416), backend=backend, device=device)
def run_model(self, img: np.ndarray):
return self.model(img)
def visualize_results(self, img: np.ndarray, result_from_run_model) -> np.ndarray:
bboxes, cls_inds = result_from_run_model
h, w = img.shape[:2]
img_area = h * w
for bbox, cls_id in zip(bboxes, cls_inds):
x1, y1, x2, y2 = map(int, bbox[:4])
area = (x2 - x1) * (y2 - y1)
# Color based on relative size (Green for small, Red for large)
ratio = min(1.0, max(0.0, np.sqrt(area / img_area)))
hue = int((1.0 - ratio) * 120)
hsv = np.uint8([[[hue, 255, 255]]])
color = tuple(map(int, cv2.cvtColor(hsv, cv2.COLOR_HSV2BGR)[0][0]))
cv2.rectangle(img, (x1, y1), (x2, y2), color, 3)
label = self.coco_classes[int(cls_id)] if int(cls_id) < len(self.coco_classes) else str(cls_id)
label = f"{label.capitalize()}"
# Render a solid background for the text
font_scale = 0.8
thickness = 2
(t_w, t_h), baseline = cv2.getTextSize(label, cv2.FONT_HERSHEY_SIMPLEX, font_scale, thickness)
y_label_bg = max(y1, t_h + 10)
cv2.rectangle(img, (x1, y_label_bg - t_h - 10), (x1 + t_w + 10, y_label_bg), color, -1)
# Draw white text over the solid background (anti-aliased)
cv2.putText(img, label, (x1 + 5, y_label_bg - 5), cv2.FONT_HERSHEY_SIMPLEX, font_scale, (255, 255, 255), thickness, cv2.LINE_AA)
return img
# Instantiate the custom model wrapper
yolox_model = YOLOXWrapper()
# Start the left camera stream passing the AI model to the pipeline
# for image_frame in stream_center_camera(ai_models_to_use=[yolox_model]):
# if image_frame is None:
# continue
# results = image_frame.ai_model_results[0]
# annotated_image = yolox_model.visualize_results(image_frame.image.copy(), results)
# cv2.namedWindow(yolox_model.name(), cv2.WINDOW_NORMAL)
# cv2.imshow(yolox_model.name(), annotated_image)
# if cv2.waitKey(1) == ord('q'):
# break
for synced_frame in stream_gripper_camera(ai_models_to_use=[yolox_model]):
if synced_frame is None:
continue
image_frame =synced_frame.left
results = image_frame.ai_model_results[0]
annotated_image = yolox_model.visualize_results(image_frame.image.copy(), results)
cv2.namedWindow(yolox_model.name(), cv2.WINDOW_NORMAL)
cv2.imshow(yolox_model.name(), annotated_image)
if cv2.waitKey(1) == ord('q'):
break
Cameras are calibrated in the factory and the calibration files are stored in the robot's home directory, under $HELLO_FLEET_PATH/$HELLO_FLEET_ID/calibration_cameras/.
If you need to re-calibrate your cameras, you can use the following tools.
First focus your camera lens using REx_camera_focus.
Then calibrate the camera intrinsics and extrinsics using REx_camera_calibrate.
The calibration process does the following:
- Calibrates the camera intrinsics using calibrate_camera_intrinsics.
This saves the calibration yaml file that contains
head_center,head_left, andhead_rightkeys with the K and D matrices, along with other information, at$HELLO_FLEET_PATH/$HELLO_FLEET_ID/calibration_cameras/calibration_rgb_head_camera.yamland a few other yaml files for ROS2 to work correctly. - Verifies the camera intrinsics using camera_intrinsics_validate_l2_distance. This uses pre-tape-measured values to verify the camera intrinsics are correct. The values are expected to vary a little across robots, but it's a good "sanity check".
- Calibrates the camera-camera extrinsics using calibrate_extrinsics_cameras.
This saves the calibration transforms as a yaml file containing
left_to_centerandright_to_centerkeys at$HELLO_FLEET_PATH/$HELLO_FLEET_ID/calibration_cameras/camera_extrinsics.yaml - Calibrates the camera-lidar extrinsics using calibrate_extrinsics_lidars.
This appends the camera-lidar extrinsics
transform_right_lidar_to_head_centerkey to the$HELLO_FLEET_PATH/$HELLO_FLEET_ID/calibration_dual_lidar/dual_lidar_calibration.yamlfile.
These values are used to estimate the distance to ArUco markers of known size using detector_aruco.py that uses cv2.solve_pnp or cv2.fisheye.solve_pnp depending on the lens type.
These values are also used by emulated_rgbd.py to create an colored point clouds and depth images using the left and right lidars, and each head camera using cv2.projectPoints or cv2.fisheye.projectPoints depending on the lens type. This also requires the lidar extrinsics to be calibrated using https://github.com/hello-robot/stretch_dual_lidar_calibration.
During execution of the calibration tools (REx_camera_calibrate for camera-lidar or camera intrinsics), progression and commands can be input using either the gamepad controller or terminal keyboard inputs:
- Gamepad Control: Tap
Xto capture a frame / unpause automatic movement, and holdX(for 3-4 seconds) to save the calibration to disk. - Keyboard Control: Type
x+ Enter to capture/unpause, types+ Enter to save/exit, and typeq+ Enter to quit without saving. This allows running calibration without a gamepad connected.
For camera-lidar calibration, you can also replay a previous session offline:
REx_camera_calibrate --extrinsics_lidar --replay_lastThis offline replay automatically bypasses the gamepad pauses and runs through all poses automatically without requiring user input.