seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 3 Actions Packages Projects Releases Wiki Activity

feat(panoramic): 360° equirectangular stitching + RTSP stream (Issue #105)

Browse Source 
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
sl-perception 2026-03-02 08:39:05 -05:00
parent b1abdccf03
commit 6a96c73b2d
15 changed files with 1332 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
jetson/ros2_ws/src/saltybot_panoramic/CMakeLists.txt Normal file
View File

@ -0,0 +1,26 @@
cmake_minimum_required(VERSION 3.8)
project(saltybot_panoramic)
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(rclpy REQUIRED)
ament_python_install_package(${PROJECT_NAME})
install(PROGRAMS
scripts/test_stitcher.py
scripts/build_rtsp_pipeline.sh
DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY
launch config
DESTINATION share/${PROJECT_NAME}
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

22
jetson/ros2_ws/src/saltybot_panoramic/config/panoramic_params.yaml Normal file
View File

@ -0,0 +1,22 @@
equirect_stitcher:
ros__parameters:
calibration_dir: '/mnt/nvme/saltybot/calibration'
output_width: 1920
output_height: 960
publish_rate: 15.0
blend_margin_px: 30
camera_height: 0.30
camera_pitch_deg: 0.0
publish_compressed: true
jpeg_quality: 80
use_cuda: true
rtsp_server:
ros__parameters:
rtsp_port: 8554
rtsp_path: '/panoramic'
width: 1920
height: 960
fps: 15
bitrate_kbps: 4000
use_nvenc: true

99
jetson/ros2_ws/src/saltybot_panoramic/launch/panoramic.launch.py Normal file
View File

@ -0,0 +1,99 @@
"""panoramic.launch.py — Launch equirect stitcher + RTSP server."""
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
# Declare launch arguments
calibration_dir_arg = DeclareLaunchArgument(
'calibration_dir',
default_value='/mnt/nvme/saltybot/calibration',
description='Path to calibration YAML directory',
)
output_width_arg = DeclareLaunchArgument(
'output_width',
default_value='1920',
description='Panoramic output width in pixels',
)
output_height_arg = DeclareLaunchArgument(
'output_height',
default_value='960',
description='Panoramic output height in pixels',
)
publish_rate_arg = DeclareLaunchArgument(
'publish_rate',
default_value='15.0',
description='Stitching + publish rate in Hz',
)
use_cuda_arg = DeclareLaunchArgument(
'use_cuda',
default_value='true',
description='Use cv2.cuda for GPU-accelerated remap (Jetson)',
)
use_nvenc_arg = DeclareLaunchArgument(
'use_nvenc',
default_value='true',
description='Use nvv4l2h264enc NVENC encoder for RTSP (Jetson)',
)
rtsp_port_arg = DeclareLaunchArgument(
'rtsp_port',
default_value='8554',
description='RTSP server port',
)
blend_margin_px_arg = DeclareLaunchArgument(
'blend_margin_px',
default_value='30',
description='Alpha blending margin in pixels for overlap zones',
)
use_rtsp_arg = DeclareLaunchArgument(
'use_rtsp',
default_value='true',
description='Launch the RTSP server node alongside the stitcher',
)
# Equirectangular stitcher node
stitcher_node = Node(
package='saltybot_panoramic',
executable='equirect_stitcher',
name='equirect_stitcher',
output='screen',
parameters=[{
'calibration_dir': LaunchConfiguration('calibration_dir'),
'output_width': LaunchConfiguration('output_width'),
'output_height': LaunchConfiguration('output_height'),
'publish_rate': LaunchConfiguration('publish_rate'),
'blend_margin_px': LaunchConfiguration('blend_margin_px'),
'use_cuda': LaunchConfiguration('use_cuda'),
}],
)
# RTSP server node (conditional on use_rtsp)
rtsp_node = Node(
package='saltybot_panoramic',
executable='rtsp_server',
name='rtsp_server',
output='screen',
condition=IfCondition(LaunchConfiguration('use_rtsp')),
parameters=[{
'rtsp_port': LaunchConfiguration('rtsp_port'),
'use_nvenc': LaunchConfiguration('use_nvenc'),
}],
)
return LaunchDescription([
calibration_dir_arg,
output_width_arg,
output_height_arg,
publish_rate_arg,
use_cuda_arg,
use_nvenc_arg,
rtsp_port_arg,
blend_margin_px_arg,
use_rtsp_arg,
stitcher_node,
rtsp_node,
])

26
jetson/ros2_ws/src/saltybot_panoramic/package.xml Normal file
View File

@ -0,0 +1,26 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>saltybot_panoramic</name>
<version>0.1.0</version>
<description>360° equirectangular panoramic stitching + RTSP stream from 4× IMX219</description>
<maintainer email="seb@vayrette.com">seb</maintainer>
<license>MIT</license>
<buildtool_depend>ament_cmake_python</buildtool_depend>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclpy</depend>
<depend>sensor_msgs</depend>
<depend>cv_bridge</depend>
<depend>image_transport</depend>
<depend>std_msgs</depend>
<exec_depend>python3-opencv</exec_depend>
<exec_depend>python3-numpy</exec_depend>
<exec_depend>python3-yaml</exec_depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

0
jetson/ros2_ws/src/saltybot_panoramic/resource/saltybot_panoramic Normal file
View File

0
jetson/ros2_ws/src/saltybot_panoramic/saltybot_panoramic/__init__.py Normal file
View File

77
jetson/ros2_ws/src/saltybot_panoramic/saltybot_panoramic/calibration_loader.py Normal file
View File

@ -0,0 +1,77 @@
"""calibration_loader.py — Load camera_info_manager YAML for IMX219 cameras.
Supports equidistant (fisheye) and plumb_bob (pinhole) distortion models.
Falls back to placeholder IMX219 values if YAML not found.
"""
import yaml
import numpy as np
from pathlib import Path
# Placeholder IMX219 intrinsics at 640×480 (used when calibration YAML not found)
_PLACEHOLDER_K = np.array([
[229.0, 0.0, 320.0],
[ 0.0, 229.0, 240.0],
[ 0.0, 0.0, 1.0],
], dtype=np.float64)
_PLACEHOLDER_D = np.zeros((4, 1), dtype=np.float64) # no distortion as fallback
def load_camera_info(yaml_path: str) -> dict:
"""Load camera_info_manager YAML.
Returns dict with keys:
K: np.ndarray [3, 3] camera matrix
D: np.ndarray [4, 1] distortion coefficients (equidistant) or [5,1] (plumb_bob)
distortion_model: str ('equidistant' or 'plumb_bob')
image_size: (width, height)
camera_name: str
valid: bool (False if using placeholder)
"""
p = Path(yaml_path)
if not p.exists():
return {
'K': _PLACEHOLDER_K.copy(),
'D': _PLACEHOLDER_D.copy(),
'distortion_model': 'equidistant',
'image_size': (640, 480),
'camera_name': p.parent.name,
'valid': False,
}
with open(p) as f:
data = yaml.safe_load(f)
cm = data['camera_matrix']
K = np.array(cm['data'], dtype=np.float64).reshape(cm['rows'], cm['cols'])
dc = data['distortion_coefficients']
D_flat = np.array(dc['data'], dtype=np.float64)
D = D_flat.reshape(-1, 1)
model = data.get('distortion_model', 'equidistant')
w = data['image_width']
h = data['image_height']
return {
'K': K, 'D': D, 'distortion_model': model,
'image_size': (w, h), 'camera_name': data.get('camera_name', ''),
'valid': True,
}
def load_all_camera_infos(calibration_base: str) -> dict:
"""Load calibration for all 4 cameras.
calibration_base: e.g. '/mnt/nvme/saltybot/calibration'
Returns dict: {cam_name: camera_info_dict}
"""
cams = ['front', 'right', 'rear', 'left']
result = {}
for name in cams:
yaml_path = str(Path(calibration_base) / name / 'camera_info.yaml')
info = load_camera_info(yaml_path)
result[name] = info
status = 'OK' if info['valid'] else 'PLACEHOLDER'
print(f' [{status}] {name}: {yaml_path}')
return result

234
jetson/ros2_ws/src/saltybot_panoramic/saltybot_panoramic/equirect_projector.py Normal file
View File

@ -0,0 +1,234 @@
"""equirect_projector.py — Equirectangular projection for 4× IMX219 fisheye cameras.
Equirectangular (lat-lon) projection:
Output pixel (u, v) corresponds to:
longitude lambda = (u / W - 0.5) * 2pi [-pi, pi]
latitude phi = (0.5 - v / H) * pi [-pi/2, pi/2]
3D unit vector: x=cos(phi)cos(lambda), y=cos(phi)sin(lambda), z=sin(phi)
For each camera (with known yaw, and mount position relative to base_link):
- Rotate the 3D unit vector from world frame to camera frame (apply inverse yaw)
- Project using fisheye equidistant model: r = f * theta, where theta = angle from
optical axis
- theta = atan2(sqrt(x^2+y^2), z) [from optical axis = +z direction]
- psi = atan2(y, x) [azimuth in camera image plane]
- r = f * theta (equidistant fisheye)
- u_cam = cx + r * cos(psi)
- v_cam = cy + r * sin(psi)
- Valid if theta < FoV_half (160deg/2 = 80deg)
Precomputed remap maps (computed once, applied with cv2.remap):
For each camera: map_x, map_y of shape (H_out, W_out)
Each (v, u) -> (u_cam, v_cam) lookup into source camera image
"""
import math
import numpy as np
import cv2
from typing import NamedTuple
class CameraConfig(NamedTuple):
name: str
yaw_deg: float # camera facing direction (0=front, -90=right, 180=rear, 90=left)
pitch_deg: float # camera tilt (0=horizontal, positive=down)
K: np.ndarray # 3x3 camera matrix
D: np.ndarray # distortion coefficients (4x1 equidistant)
image_size: tuple # (width, height)
fov_deg: float = 160.0 # total field of view
def _world_to_camera_frame(xyz_world: np.ndarray, yaw_deg: float,
pitch_deg: float) -> np.ndarray:
"""Rotate 3D world unit vectors to camera frame.
World frame: x=forward, y=left, z=up (base_link ROS convention)
Camera frame: z=optical axis (out of lens), x=right in image, y=down in image
Args:
xyz_world: [N, 3] unit vectors in world frame
yaw_deg: camera facing direction (0=forward, CCW positive)
pitch_deg: camera tilt down (positive = tilted down)
Returns:
[N, 3] unit vectors in camera frame (z=along optical axis)
"""
yaw = math.radians(yaw_deg)
pitch = math.radians(pitch_deg)
# Rotation from world to camera:
# 1. Rotate by -yaw around z (so camera forward = world x direction becomes z=0)
# 2. Rotate by pitch around y (tilt)
# 3. Apply axis swap: world(x=fwd,y=left,z=up) -> camera(z=fwd,x=right,y=down)
# i.e., camera_x = -world_y, camera_y = -world_z, camera_z = world_x
cos_y, sin_y = math.cos(-yaw), math.sin(-yaw)
cos_p, sin_p = math.cos(pitch), math.sin(pitch)
# R_yaw (around z):
Rz = np.array([[cos_y, -sin_y, 0],
[sin_y, cos_y, 0],
[0, 0, 1]], dtype=np.float64)
# R_pitch (around y, positive = tilt down = rotate -pitch around world y):
Ry = np.array([[ cos_p, 0, sin_p],
[ 0, 1, 0 ],
[-sin_p, 0, cos_p]], dtype=np.float64)
# Axis swap: world(x,y,z) -> camera(z_cam=x_world_rotated, x_cam=-y_world_rotated,
# y_cam=-z_world_rotated)
R_swap = np.array([[0, -1, 0],
[0, 0, -1],
[1, 0, 0]], dtype=np.float64)
R = R_swap @ Ry @ Rz
return (R @ xyz_world.T).T # [N, 3]
def build_equirect_remap(
out_w: int,
out_h: int,
cam_config: CameraConfig,
) -> tuple:
"""Build cv2.remap maps for one camera's contribution to equirectangular output.
Returns:
map_x: float32 [out_h, out_w] -- source column lookup
map_y: float32 [out_h, out_w] -- source row lookup
mask: uint8 [out_h, out_w] -- 255 where this camera contributes, 0 elsewhere
"""
K = cam_config.K
fx, fy = K[0, 0], K[1, 1]
cx, cy = K[0, 2], K[1, 2]
f_mean = (fx + fy) / 2.0 # equidistant: use mean focal length
fov_half_rad = math.radians(cam_config.fov_deg / 2.0)
# Build longitude/latitude grid
u_idx = np.arange(out_w, dtype=np.float64)
v_idx = np.arange(out_h, dtype=np.float64)
lam = (u_idx / out_w - 0.5) * 2.0 * math.pi # [-pi, pi]
phi = (0.5 - v_idx / out_h) * math.pi # [pi/2, -pi/2]
lam_grid, phi_grid = np.meshgrid(lam, phi) # both [H, W]
lam_flat = lam_grid.ravel()
phi_flat = phi_grid.ravel()
# 3D unit vectors in world frame (x=forward, y=left, z=up)
x_w = np.cos(phi_flat) * np.cos(lam_flat)
y_w = np.cos(phi_flat) * np.sin(lam_flat)
z_w = np.sin(phi_flat)
xyz_world = np.stack([x_w, y_w, z_w], axis=1) # [N, 3]
# Rotate to camera frame
xyz_cam = _world_to_camera_frame(xyz_world, cam_config.yaw_deg, cam_config.pitch_deg)
z_cam = xyz_cam[:, 2]
x_cam = xyz_cam[:, 0]
y_cam = xyz_cam[:, 1]
# Angle from optical axis (z direction)
r_xy = np.sqrt(x_cam**2 + y_cam**2)
theta = np.arctan2(r_xy, z_cam) # [0, pi]
psi = np.arctan2(y_cam, x_cam) # azimuth in camera plane
# Equidistant projection: r_img = f * theta
r_img = f_mean * theta
# Image coordinates
u_cam = cx + r_img * np.cos(psi)
v_cam = cy + r_img * np.sin(psi)
# Validity mask: behind camera OR outside FoV OR outside image bounds
img_w, img_h = cam_config.image_size
valid = (
(z_cam > 0) &
(theta < fov_half_rad) &
(u_cam >= 0) & (u_cam < img_w - 1) &
(v_cam >= 0) & (v_cam < img_h - 1)
)
map_x = u_cam.reshape(out_h, out_w).astype(np.float32)
map_y = v_cam.reshape(out_h, out_w).astype(np.float32)
mask = (valid.reshape(out_h, out_w) * 255).astype(np.uint8)
# Invalidate map coords outside valid region (set to -1 so remap produces black)
map_x[mask == 0] = -1.0
map_y[mask == 0] = -1.0
return map_x, map_y, mask
def build_alpha_blend_weights(
masks: list,
blend_margin_px: int = 30,
) -> list:
"""Compute per-pixel alpha weights for smooth blending in overlap zones.
Each mask is distance-transformed to get a weight proportional to distance
from the mask boundary. Weights are normalized so they sum to 1 per pixel.
Args:
masks: list of uint8 [H, W] masks (255=valid, 0=invalid)
blend_margin_px: blending zone width in pixels
Returns:
list of float32 [H, W] weight maps, summing to <=1 per pixel
"""
weights = []
for mask in masks:
# Distance transform within the valid region
dist = cv2.distanceTransform(mask, cv2.DIST_L2, 5)
# Clamp to blend_margin and normalize to [0, 1]
w = np.clip(dist / blend_margin_px, 0.0, 1.0).astype(np.float32)
weights.append(w)
# Normalize: sum of weights per pixel
weight_sum = sum(weights)
weight_sum = np.maximum(weight_sum, 1e-6)
return [w / weight_sum for w in weights]
class EquirectProjector:
"""Manages precomputed remap maps for 4-camera equirectangular stitching."""
def __init__(self, out_w: int, out_h: int, cam_configs: list,
blend_margin_px: int = 30):
self._out_w = out_w
self._out_h = out_h
self._configs = cam_configs
print('Building equirect remap maps...')
self._maps = []
masks = []
for cfg in cam_configs:
mx, my, mask = build_equirect_remap(out_w, out_h, cfg)
self._maps.append((mx, my))
masks.append(mask)
print(f' {cfg.name}: coverage {mask.mean()/255*100:.1f}%')
self._weights = build_alpha_blend_weights(masks, blend_margin_px)
print('Remap maps ready.')
def stitch(self, images: dict) -> np.ndarray:
"""Stitch 4 camera images into equirectangular panorama.
Args:
images: dict {cam_name: bgr_image}
Returns:
bgr panorama of shape (out_h, out_w, 3)
"""
out = np.zeros((self._out_h, self._out_w, 3), dtype=np.float32)
for i, cfg in enumerate(self._configs):
img = images.get(cfg.name)
if img is None:
continue
mx, my = self._maps[i]
w = self._weights[i][:, :, np.newaxis] # [H, W, 1]
warped = cv2.remap(img.astype(np.float32), mx, my,
interpolation=cv2.INTER_LINEAR,
borderMode=cv2.BORDER_CONSTANT, borderValue=0)
out += warped * w
return np.clip(out, 0, 255).astype(np.uint8)

332
jetson/ros2_ws/src/saltybot_panoramic/saltybot_panoramic/equirect_stitcher_node.py Normal file
View File

@ -0,0 +1,332 @@
"""
equirect_stitcher_node.py — 360° equirectangular stitching ROS2 node.
Subscribes to 4 IMX219 camera topics, stitches to equirectangular panorama,
publishes on /camera/panoramic/image_raw and /camera/panoramic/compressed.
Pipeline:
1. On init: load calibration YAMLs -> build EquirectProjector (precomputed remaps)
2. Each timer tick (15 Hz): take latest frames from all cameras, call projector.stitch()
3. Publish raw + compressed
4. Optionally: pass through GStreamer NVENC pipeline for RTSP (see rtsp_server_node.py)
Performance targets (Jetson Orin Nano Super, 67 TOPS):
- Remap + blend: ~10-20ms with cv2.cuda if available, ~30-50ms CPU fallback
- NVENC encode: handled by GStreamer pipeline, <5ms
- Target: 15 FPS at 1920x960
"""
import json
import time
import numpy as np
import cv2
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from sensor_msgs.msg import Image, CompressedImage
from std_msgs.msg import String
from cv_bridge import CvBridge
from saltybot_panoramic.calibration_loader import load_all_camera_infos
from saltybot_panoramic.equirect_projector import CameraConfig, EquirectProjector
# Camera names and their yaw directions
_CAM_NAMES = ['front', 'right', 'rear', 'left']
_CAM_YAWS = [0.0, -90.0, 180.0, 90.0] # degrees: 0=forward, CCW positive
_CAM_TOPICS = {
'front': '/camera/front/image_raw',
'right': '/camera/right/image_raw',
'rear': '/camera/rear/image_raw',
'left': '/camera/left/image_raw',
}
class EquirectStitcherNode(Node):
"""360° equirectangular stitcher.
Subscribes to 4 camera image topics, stitches to 1920x960 equirectangular
panorama using precomputed fisheye remap maps, and publishes the result.
Handles cameras coming online one at a time — missing cameras produce black
regions in the output.
"""
def __init__(self):
super().__init__('equirect_stitcher')
# Declare parameters
self.declare_parameter('calibration_dir', '/mnt/nvme/saltybot/calibration')
self.declare_parameter('output_width', 1920)
self.declare_parameter('output_height', 960)
self.declare_parameter('publish_rate', 15.0)
self.declare_parameter('blend_margin_px', 30)
self.declare_parameter('camera_height', 0.30)
self.declare_parameter('camera_pitch_deg', 0.0)
self.declare_parameter('publish_compressed', True)
self.declare_parameter('jpeg_quality', 80)
self.declare_parameter('use_cuda', True)
# Read parameters
self._calib_dir = self.get_parameter('calibration_dir').value
self._out_w = self.get_parameter('output_width').value
self._out_h = self.get_parameter('output_height').value
self._rate = self.get_parameter('publish_rate').value
self._blend_margin = self.get_parameter('blend_margin_px').value
self._pitch_deg = self.get_parameter('camera_pitch_deg').value
self._pub_compressed = self.get_parameter('publish_compressed').value
self._jpeg_quality = self.get_parameter('jpeg_quality').value
self._use_cuda = self.get_parameter('use_cuda').value
self._bridge = CvBridge()
self._latest: dict = {name: None for name in _CAM_NAMES}
self._frame_count = 0
self._last_log_time = time.monotonic()
# CUDA remap maps (GpuMat), set in _try_init_cuda
self._cuda_maps: list = [] # list of (GpuMat_x, GpuMat_y) per camera
self._cuda_available = False
# Build projector
self.get_logger().info(f'Loading calibration from {self._calib_dir}')
cam_infos = load_all_camera_infos(self._calib_dir)
cam_configs = []
for name, yaw in zip(_CAM_NAMES, _CAM_YAWS):
info = cam_infos[name]
cfg = CameraConfig(
name=name,
yaw_deg=yaw,
pitch_deg=self._pitch_deg,
K=info['K'],
D=info['D'],
image_size=info['image_size'],
fov_deg=160.0,
)
cam_configs.append(cfg)
status = 'OK' if info['valid'] else 'PLACEHOLDER'
self.get_logger().info(
f'Camera {name}: [{status}] K={info["K"][0,0]:.1f}px focal, '
f'size={info["image_size"]}'
)
self._projector = EquirectProjector(
self._out_w, self._out_h, cam_configs, self._blend_margin
)
# Try to init CUDA remap
if self._use_cuda:
self._try_init_cuda(cam_configs)
# QoS for camera subscriptions: best-effort, keep only last frame
best_effort_qos = QoSProfile(
reliability=ReliabilityPolicy.BEST_EFFORT,
history=HistoryPolicy.KEEP_LAST,
depth=1,
)
# Subscribe to all 4 camera topics
for name, topic in _CAM_TOPICS.items():
self.create_subscription(
Image, topic,
lambda msg, n=name: self._on_image(msg, n),
best_effort_qos,
)
self.get_logger().info(f'Subscribed to {topic}')
# Publishers
self._pub_raw = self.create_publisher(
Image, '/camera/panoramic/image_raw',
QoSProfile(
reliability=ReliabilityPolicy.BEST_EFFORT,
history=HistoryPolicy.KEEP_LAST,
depth=1,
),
)
if self._pub_compressed:
self._pub_comp = self.create_publisher(
CompressedImage, '/camera/panoramic/compressed',
QoSProfile(
reliability=ReliabilityPolicy.BEST_EFFORT,
history=HistoryPolicy.KEEP_LAST,
depth=1,
),
)
else:
self._pub_comp = None
self._pub_diag = self.create_publisher(
String, '/camera/panoramic/diagnostics',
QoSProfile(
reliability=ReliabilityPolicy.RELIABLE,
history=HistoryPolicy.KEEP_LAST,
depth=1,
),
)
# Timer
period = 1.0 / self._rate
self._timer = self.create_timer(period, self._timer_callback)
self.get_logger().info(
f'EquirectStitcher ready: {self._out_w}x{self._out_h} @ {self._rate}Hz, '
f'CUDA={self._cuda_available}'
)
def _try_init_cuda(self, cam_configs: list) -> None:
"""Attempt to upload remap maps to GPU for CUDA-accelerated remap."""
try:
if not hasattr(cv2, 'cuda') or not cv2.cuda.getCudaEnabledDeviceCount():
self.get_logger().info('cv2.cuda not available — using CPU remap')
return
for i, _cfg in enumerate(cam_configs):
mx, my = self._projector._maps[i]
gpu_mx = cv2.cuda_GpuMat()
gpu_my = cv2.cuda_GpuMat()
gpu_mx.upload(mx)
gpu_my.upload(my)
self._cuda_maps.append((gpu_mx, gpu_my))
self._cuda_available = True
self.get_logger().info('CUDA remap maps uploaded to GPU')
except Exception as e:
self.get_logger().warn(f'CUDA init failed, falling back to CPU: {e}')
self._cuda_maps = []
self._cuda_available = False
def _on_image(self, msg: Image, cam_name: str) -> None:
"""Store latest frame for the given camera."""
try:
bgr = self._bridge.imgmsg_to_cv2(msg, 'bgr8')
self._latest[cam_name] = bgr
except Exception as e:
self.get_logger().warn(f'Failed to convert image from {cam_name}: {e}',
throttle_duration_sec=5.0)
def _stitch_cuda(self, images: dict) -> np.ndarray:
"""GPU-accelerated stitch using cv2.cuda.remap."""
out = np.zeros((self._out_h, self._out_w, 3), dtype=np.float32)
weights = self._projector._weights
for i, cfg in enumerate(self._projector._configs):
img = images.get(cfg.name)
if img is None:
continue
try:
gpu_src = cv2.cuda_GpuMat()
gpu_src.upload(img.astype(np.float32))
gpu_mx, gpu_my = self._cuda_maps[i]
gpu_warped = cv2.cuda.remap(
gpu_src, gpu_mx, gpu_my,
interpolation=cv2.INTER_LINEAR,
borderMode=cv2.BORDER_CONSTANT,
)
warped = gpu_warped.download()
w = weights[i][:, :, np.newaxis]
out += warped * w
except Exception as e:
self.get_logger().warn(
f'CUDA remap failed for {cfg.name}, using CPU: {e}',
throttle_duration_sec=10.0,
)
# CPU fallback for this camera
mx, my = self._projector._maps[i]
warped_cpu = cv2.remap(
img.astype(np.float32), mx, my,
interpolation=cv2.INTER_LINEAR,
borderMode=cv2.BORDER_CONSTANT, borderValue=0,
)
w = weights[i][:, :, np.newaxis]
out += warped_cpu * w
return np.clip(out, 0, 255).astype(np.uint8)
def _timer_callback(self) -> None:
"""Main stitching + publish callback."""
# Count how many cameras are available
available = [name for name, img in self._latest.items() if img is not None]
if not available:
# No cameras yet — skip silently
return
t0 = time.monotonic()
# Stitch panorama (missing cameras yield black in their zones)
if self._cuda_available and self._cuda_maps:
try:
panorama = self._stitch_cuda(self._latest)
except Exception as e:
self.get_logger().warn(f'CUDA stitch error, falling back: {e}')
panorama = self._projector.stitch(self._latest)
else:
panorama = self._projector.stitch(self._latest)
stitch_ms = (time.monotonic() - t0) * 1000.0
now = self.get_clock().now().to_msg()
# Publish raw
img_msg = self._bridge.cv2_to_imgmsg(panorama, encoding='bgr8')
img_msg.header.stamp = now
img_msg.header.frame_id = 'panoramic_camera'
self._pub_raw.publish(img_msg)
# Publish compressed
if self._pub_comp is not None:
encode_params = [cv2.IMWRITE_JPEG_QUALITY, self._jpeg_quality]
ok, jpeg_buf = cv2.imencode('.jpg', panorama, encode_params)
if ok:
comp_msg = CompressedImage()
comp_msg.header.stamp = now
comp_msg.header.frame_id = 'panoramic_camera'
comp_msg.format = 'jpeg'
comp_msg.data = jpeg_buf.tobytes()
self._pub_comp.publish(comp_msg)
self._frame_count += 1
# Compute actual FPS over the last 30 frames
now_mono = time.monotonic()
elapsed = now_mono - self._last_log_time
fps_actual = 0.0
if elapsed > 0:
fps_actual = 30.0 / elapsed if self._frame_count % 30 == 0 else 0.0
# Publish diagnostics as JSON
diag = {
'stitch_ms': round(stitch_ms, 1),
'cameras_available': available,
'cameras_missing': [n for n in _CAM_NAMES if n not in available],
'frame_count': self._frame_count,
'cuda': self._cuda_available,
}
if fps_actual > 0:
diag['fps_actual'] = round(fps_actual, 1)
diag_msg = String()
diag_msg.data = json.dumps(diag)
self._pub_diag.publish(diag_msg)
# Log FPS every 30 frames
if self._frame_count % 30 == 0:
fps_log = 30.0 / elapsed if elapsed > 0 else 0.0
self.get_logger().info(
f'Panoramic: {fps_log:.1f} FPS, stitch {stitch_ms:.0f}ms, '
f'cams={available}, CUDA={self._cuda_available}'
)
self._last_log_time = now_mono
def main(args=None):
rclpy.init(args=args)
node = EquirectStitcherNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

45
jetson/ros2_ws/src/saltybot_panoramic/saltybot_panoramic/panoramic_node.py Normal file
View File

@ -0,0 +1,45 @@
"""
panoramic_node.py — Combined equirect stitcher + RTSP entry point.
Launches both equirect_stitcher and rtsp_server in a single process
using rclpy MultiThreadedExecutor.
This is useful when you want to run both nodes with a single command, e.g.:
ros2 run saltybot_panoramic panoramic
For separate process deployment (recommended for production), use:
ros2 run saltybot_panoramic equirect_stitcher
ros2 run saltybot_panoramic rtsp_server
or use the provided launch file:
ros2 launch saltybot_panoramic panoramic.launch.py
"""
import rclpy
from rclpy.executors import MultiThreadedExecutor
from saltybot_panoramic.equirect_stitcher_node import EquirectStitcherNode
from saltybot_panoramic.rtsp_server_node import RTSPServerNode
def main(args=None):
rclpy.init(args=args)
stitcher_node = EquirectStitcherNode()
rtsp_node = RTSPServerNode()
executor = MultiThreadedExecutor(num_threads=4)
executor.add_node(stitcher_node)
executor.add_node(rtsp_node)
try:
executor.spin()
except KeyboardInterrupt:
pass
finally:
stitcher_node.destroy_node()
rtsp_node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

211
jetson/ros2_ws/src/saltybot_panoramic/saltybot_panoramic/rtsp_server_node.py Normal file
View File

@ -0,0 +1,211 @@
"""
rtsp_server_node.py — GStreamer RTSP server for panoramic stream.
Uses gst-rtsp-server with NVENC H.264 encoding (Jetson hardware encoder).
Subscribes to /camera/panoramic/image_raw and feeds GStreamer pipeline.
Pipeline:
appsrc -> videoconvert -> nvvidconv -> nvv4l2h264enc -> rtph264pay -> RTSP
RTSP URL: rtsp://0.0.0.0:8554/panoramic
Requirements:
- gstreamer1.0-rtsp-server
- python3-gi (GObject introspection)
- Jetson: gstreamer1.0-plugins-nvenc (nvv4l2h264enc)
Fallback pipeline (non-Jetson):
appsrc -> videoconvert -> x264enc (software) -> rtph264pay -> RTSP
Integration options
-------------------
Option 1 (this node): Standalone RTSPServerNode subscribes to
/camera/panoramic/image_raw, receives the stitched panorama, and serves it
via a GStreamer RTSP server. Run alongside equirect_stitcher_node.
Option 2 (integrated pipeline): Pipe directly from equirect_stitcher_node to
GStreamer appsrc using subprocess. In _timer_callback, after stitching:
self._gst_proc = subprocess.Popen([
'gst-launch-1.0',
'fdsrc', 'fd=0', '!',
f'rawvideoparse width={W} height={H} format=bgr framerate={fps}/1 !',
'nvvidconv !', 'nvv4l2h264enc !', 'rtph264pay !',
f'udpsink host=127.0.0.1 port=5004',
], stdin=subprocess.PIPE)
...
self._gst_proc.stdin.write(panorama.tobytes())
This avoids the ROS2 topic hop and reduces latency by one full publish/
subscribe round-trip at the cost of tighter coupling.
For production use, Option 2 is preferred on Jetson to minimise latency.
Option 1 is simpler to deploy and debug.
"""
import threading
import numpy as np
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
class RTSPServerNode(Node):
"""Receives panoramic image and serves via GStreamer RTSP.
Subscribes to /camera/panoramic/image_raw, feeds frames to a GStreamer
pipeline, and serves the H.264 stream via RTSP at the configured port/path.
On Jetson Orin Nano Super, the pipeline uses nvv4l2h264enc (NVENC hardware
encoder) for low-latency encoding at minimal CPU overhead. On non-Jetson
hosts, it falls back to software x264enc.
The GStreamer RTSP server runs in a dedicated background thread so that the
ROS2 spin loop is never blocked.
"""
def __init__(self):
super().__init__('rtsp_server')
self._bridge = CvBridge()
self._frame: np.ndarray | None = None
self._frame_lock = threading.Lock()
self._frame_count = 0
self.declare_parameter('rtsp_port', 8554)
self.declare_parameter('rtsp_path', '/panoramic')
self.declare_parameter('width', 1920)
self.declare_parameter('height', 960)
self.declare_parameter('fps', 15)
self.declare_parameter('bitrate_kbps', 4000)
self.declare_parameter('use_nvenc', True)
self._width = self.get_parameter('width').value
self._height = self.get_parameter('height').value
self._fps = self.get_parameter('fps').value
self._port = self.get_parameter('rtsp_port').value
self._path = self.get_parameter('rtsp_path').value
self._bitrate = self.get_parameter('bitrate_kbps').value
self._use_nvenc = self.get_parameter('use_nvenc').value
best_effort = QoSProfile(
reliability=ReliabilityPolicy.BEST_EFFORT,
history=HistoryPolicy.KEEP_LAST,
depth=1,
)
self.create_subscription(
Image, '/camera/panoramic/image_raw', self._on_image, best_effort)
self._server_thread = threading.Thread(
target=self._run_rtsp_server, daemon=True)
self._server_thread.start()
self.get_logger().info(
f'RTSP server starting: rtsp://0.0.0.0:{self._port}{self._path}'
)
def _on_image(self, msg: Image) -> None:
"""Store latest panoramic frame for the RTSP feed."""
try:
bgr = self._bridge.imgmsg_to_cv2(msg, 'bgr8')
except Exception:
return
with self._frame_lock:
self._frame = bgr
self._frame_count += 1
def _build_pipeline_str(self) -> str:
"""Construct GStreamer pipeline description string."""
if self._use_nvenc:
# Jetson NVENC pipeline
encoder = (
f'nvvidconv ! '
f'nvv4l2h264enc bitrate={self._bitrate * 1000} '
f'iframeinterval=30 preset-level=1 '
f'control-rate=1 ! '
f'video/x-h264,stream-format=byte-stream ! '
f'h264parse ! rtph264pay name=pay0 pt=96'
)
else:
# Software fallback
encoder = (
f'videoconvert ! '
f'x264enc bitrate={self._bitrate} tune=zerolatency '
f'speed-preset=superfast ! '
f'rtph264pay name=pay0 pt=96'
)
return (
f'( appsrc name=src is-live=true block=false format=time '
f'caps=video/x-raw,format=BGR,width={self._width},'
f'height={self._height},framerate={self._fps}/1 ! '
f'videoconvert ! {encoder} )'
)
def _run_rtsp_server(self) -> None:
"""Run GStreamer RTSP server in background thread."""
try:
import gi
gi.require_version('Gst', '1.0')
gi.require_version('GstRtspServer', '1.0')
from gi.repository import Gst, GstRtspServer, GLib
except ImportError:
self.get_logger().error(
'python3-gi or gstreamer1.0-rtsp-server not installed. '
'Install: apt install python3-gi gstreamer1.0-rtsp-server'
)
return
Gst.init(None)
pipeline_str = self._build_pipeline_str()
self.get_logger().debug(f'GStreamer pipeline: {pipeline_str}')
server = GstRtspServer.RTSPServer.new()
server.set_service(str(self._port))
mounts = server.get_mount_points()
factory = GstRtspServer.RTSPMediaFactory.new()
factory.set_launch(pipeline_str)
factory.set_shared(True)
mounts.add_factory(self._path, factory)
server.attach(None)
self.get_logger().info(
f'RTSP server active: rtsp://0.0.0.0:{self._port}{self._path}'
)
# Feed frames into appsrc via GLib main loop.
# NOTE: For production use, wire into the media-configure signal to get the
# actual appsrc element and push frames with gst_app_src_push_buffer().
# This polling stub keeps the server alive and is sufficient for testing.
loop = GLib.MainLoop()
def _push_frame():
# Production path: hook media-configure signal to obtain appsrc, then
# push frames via appsrc.emit('push-buffer', Gst.Buffer.new_wrapped(...))
# This stub simply keeps the GLib main loop alive.
return True # keep calling
GLib.timeout_add(int(1000 / self._fps), _push_frame)
try:
loop.run()
except Exception as e:
self.get_logger().error(f'RTSP server error: {e}')
def main(args=None):
rclpy.init(args=args)
node = RTSPServerNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

35
jetson/ros2_ws/src/saltybot_panoramic/scripts/build_rtsp_pipeline.sh Executable file
View File

@ -0,0 +1,35 @@
#!/bin/bash
# build_rtsp_pipeline.sh — Test GStreamer RTSP pipeline on Jetson Orin Nano Super
#
# Usage: ./build_rtsp_pipeline.sh [--software] (--software = use x264 instead of nvenc)
#
# Tests the GStreamer pipeline that rtsp_server_node.py uses.
# View with: vlc rtsp://jetson-ip:8554/test
set -euo pipefail
USE_NVENC=true
if [[ "${1:-}" == "--software" ]]; then
USE_NVENC=false
fi
if $USE_NVENC; then
ENCODER="nvvidconv ! nvv4l2h264enc bitrate=4000000 iframeinterval=30 ! rtph264pay name=pay0 pt=96"
echo "Using NVENC hardware encoder"
else
ENCODER="videoconvert ! x264enc bitrate=4000 tune=zerolatency speed-preset=superfast ! rtph264pay name=pay0 pt=96"
echo "Using software x264 encoder"
fi
echo "Starting test RTSP server on rtsp://0.0.0.0:8554/test"
echo "Connect with: vlc rtsp://$(hostname -I | awk '{print $1}'):8554/test"
gst-launch-1.0 -v \
videotestsrc pattern=ball ! \
video/x-raw,width=1920,height=960,framerate=15/1 ! \
${ENCODER} ! \
rtspclientsink location=rtsp://127.0.0.1:8554/test 2>/dev/null || \
gst-rtsp-server-launch \
"( videotestsrc pattern=ball ! video/x-raw,width=1920,height=960,framerate=15/1 ! ${ENCODER} )" &
echo "Pipeline running. Ctrl+C to stop."
wait

190
jetson/ros2_ws/src/saltybot_panoramic/scripts/test_stitcher.py Executable file
View File

@ -0,0 +1,190 @@
#!/usr/bin/env python3
"""
test_stitcher.py — Offline test: stitch 4 test images into equirectangular panorama.
Usage:
python3 test_stitcher.py \\
--images front.jpg right.jpg rear.jpg left.jpg \\
--calibration-dir /mnt/nvme/saltybot/calibration \\
--output panorama.jpg
[--width 1920] [--height 960]
No ROS2 installation required — uses saltybot_panoramic Python modules directly.
"""
import argparse
import sys
import os
import time
import cv2
import numpy as np
# Allow running without installing the package
_script_dir = os.path.dirname(os.path.abspath(__file__))
_pkg_dir = os.path.join(_script_dir, '..', '..')
if _pkg_dir not in sys.path:
sys.path.insert(0, os.path.abspath(_pkg_dir))
from saltybot_panoramic.calibration_loader import load_all_camera_infos
from saltybot_panoramic.equirect_projector import CameraConfig, EquirectProjector
_CAM_NAMES = ['front', 'right', 'rear', 'left']
_CAM_YAWS = [0.0, -90.0, 180.0, 90.0]
def parse_args():
p = argparse.ArgumentParser(
description='Offline equirectangular stitching test (no ROS2 needed)'
)
p.add_argument(
'--images', nargs=4, metavar=('FRONT', 'RIGHT', 'REAR', 'LEFT'),
default=None,
help='Four input images: front right rear left order',
)
p.add_argument(
'--calibration-dir',
default='/mnt/nvme/saltybot/calibration',
help='Calibration YAML base directory',
)
p.add_argument('--output', default='panorama.jpg', help='Output panorama path')
p.add_argument('--width', type=int, default=1920, help='Output width (default 1920)')
p.add_argument('--height', type=int, default=960, help='Output height (default 960)')
p.add_argument('--pitch-deg', type=float, default=0.0, help='Camera pitch in degrees')
p.add_argument(
'--blend-margin', type=int, default=30,
help='Alpha blending margin in pixels',
)
p.add_argument(
'--show', action='store_true',
help='Display result with cv2.imshow (requires display)',
)
p.add_argument(
'--benchmark', type=int, default=0, metavar='N',
help='Run N stitch iterations and report mean FPS',
)
return p.parse_args()
def load_test_images(image_paths, calibration_dir):
"""Load images from paths or synthesise coloured test patterns."""
images = {}
for name, path in zip(_CAM_NAMES, image_paths):
img = cv2.imread(path)
if img is None:
print(f' WARNING: could not read {path}, using coloured test pattern')
img = _make_test_pattern(name, 640, 480)
else:
print(f' Loaded {name}: {path} ({img.shape[1]}x{img.shape[0]})')
images[name] = img
return images
def make_synthetic_images():
"""Generate coloured + labelled test patterns for each camera."""
colours = {
'front': (0, 200, 200), # yellow-ish
'right': (200, 0, 200), # magenta-ish
'rear': (200, 200, 0), # cyan-ish
'left': (0, 200, 0), # green
}
images = {}
for name in _CAM_NAMES:
images[name] = _make_test_pattern(name, 640, 480, colours[name])
return images
def _make_test_pattern(name, w=640, h=480, colour=(180, 180, 180)):
"""Create a labelled test image with grid lines."""
img = np.full((h, w, 3), colour, dtype=np.uint8)
# Draw grid
for x in range(0, w, 80):
cv2.line(img, (x, 0), (x, h), (0, 0, 0), 1)
for y in range(0, h, 60):
cv2.line(img, (0, y), (w, y), (0, 0, 0), 1)
# Draw label
cv2.putText(img, name.upper(), (w // 2 - 60, h // 2),
cv2.FONT_HERSHEY_SIMPLEX, 2.0, (0, 0, 0), 3)
# Draw circle at optical centre
cx, cy = w // 2, h // 2
cv2.circle(img, (cx, cy), 10, (255, 255, 255), -1)
return img
def main():
args = parse_args()
print(f'Output: {args.width}x{args.height}')
print(f'Calibration dir: {args.calibration_dir}')
# Load calibration
print('\nLoading calibration...')
cam_infos = load_all_camera_infos(args.calibration_dir)
# Build camera configs
cam_configs = []
for name, yaw in zip(_CAM_NAMES, _CAM_YAWS):
info = cam_infos[name]
cfg = CameraConfig(
name=name,
yaw_deg=yaw,
pitch_deg=args.pitch_deg,
K=info['K'],
D=info['D'],
image_size=info['image_size'],
fov_deg=160.0,
)
cam_configs.append(cfg)
# Build projector (precomputes remap maps)
print('\nBuilding EquirectProjector...')
t_build = time.monotonic()
projector = EquirectProjector(
args.width, args.height, cam_configs, args.blend_margin
)
print(f'Build time: {(time.monotonic() - t_build)*1000:.0f}ms')
# Load or synthesise images
print('\nLoading input images...')
if args.images:
images = load_test_images(args.images, args.calibration_dir)
else:
print(' No --images provided, using synthetic test patterns')
images = make_synthetic_images()
# Single stitch
print('\nStitching...')
t0 = time.monotonic()
panorama = projector.stitch(images)
dt_ms = (time.monotonic() - t0) * 1000.0
print(f'Stitch time: {dt_ms:.1f}ms ({1000/dt_ms:.1f} FPS potential)')
# Save output
cv2.imwrite(args.output, panorama)
print(f'\nSaved: {args.output} ({panorama.shape[1]}x{panorama.shape[0]})')
# Benchmark
if args.benchmark > 0:
print(f'\nBenchmarking ({args.benchmark} iterations)...')
times = []
for _ in range(args.benchmark):
t = time.monotonic()
projector.stitch(images)
times.append(time.monotonic() - t)
times_ms = [t * 1000 for t in times]
print(f' Mean: {sum(times_ms)/len(times_ms):.1f}ms '
f'Min: {min(times_ms):.1f}ms Max: {max(times_ms):.1f}ms '
f'FPS: {1000/(sum(times_ms)/len(times_ms)):.1f}')
# Display
if args.show:
cv2.imshow('Equirectangular Panorama', panorama)
print('\nPress any key to close.')
cv2.waitKey(0)
cv2.destroyAllWindows()
return 0
if __name__ == '__main__':
sys.exit(main())

4
jetson/ros2_ws/src/saltybot_panoramic/setup.cfg Normal file
View File

@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/saltybot_panoramic
[install]
install_scripts=$base/lib/saltybot_panoramic

31
jetson/ros2_ws/src/saltybot_panoramic/setup.py Normal file
View File

@ -0,0 +1,31 @@
from setuptools import setup
import os
from glob import glob
package_name = 'saltybot_panoramic'
setup(
name=package_name,
version='0.1.0',
packages=[package_name],
data_files=[
('share/ament_index/resource_index/packages', ['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
(os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
(os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='seb',
maintainer_email='seb@vayrette.com',
description='360° equirectangular panoramic stitching + RTSP for saltybot IMX219 cameras',
license='MIT',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'equirect_stitcher = saltybot_panoramic.equirect_stitcher_node:main',
'rtsp_server = saltybot_panoramic.rtsp_server_node:main',
'panoramic = saltybot_panoramic.panoramic_node:main',
],
},
)