Merge pull request 'feat(panoramic): 360° equirectangular stitching + RTSP #105' (#115) from sl-perception/issue-105-equirect into main
This commit is contained in:
commit
6a30b20aaa
26
jetson/ros2_ws/src/saltybot_panoramic/CMakeLists.txt
Normal file
26
jetson/ros2_ws/src/saltybot_panoramic/CMakeLists.txt
Normal 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()
|
||||||
@ -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
|
||||||
@ -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
26
jetson/ros2_ws/src/saltybot_panoramic/package.xml
Normal 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,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
|
||||||
@ -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)
|
||||||
@ -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()
|
||||||
@ -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()
|
||||||
@ -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
35
jetson/ros2_ws/src/saltybot_panoramic/scripts/build_rtsp_pipeline.sh
Executable 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
190
jetson/ros2_ws/src/saltybot_panoramic/scripts/test_stitcher.py
Executable 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
4
jetson/ros2_ws/src/saltybot_panoramic/setup.cfg
Normal 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
31
jetson/ros2_ws/src/saltybot_panoramic/setup.py
Normal 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',
|
||||||
|
],
|
||||||
|
},
|
||||||
|
)
|
||||||
Loading…
x
Reference in New Issue
Block a user