diff --git a/jetson/ros2_ws/src/saltybot_panoramic/CMakeLists.txt b/jetson/ros2_ws/src/saltybot_panoramic/CMakeLists.txt new file mode 100644 index 0000000..b5f9cdd --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_panoramic/CMakeLists.txt @@ -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() diff --git a/jetson/ros2_ws/src/saltybot_panoramic/config/panoramic_params.yaml b/jetson/ros2_ws/src/saltybot_panoramic/config/panoramic_params.yaml new file mode 100644 index 0000000..4993d7b --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_panoramic/config/panoramic_params.yaml @@ -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 diff --git a/jetson/ros2_ws/src/saltybot_panoramic/launch/panoramic.launch.py b/jetson/ros2_ws/src/saltybot_panoramic/launch/panoramic.launch.py new file mode 100644 index 0000000..48aaff4 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_panoramic/launch/panoramic.launch.py @@ -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, + ]) diff --git a/jetson/ros2_ws/src/saltybot_panoramic/package.xml b/jetson/ros2_ws/src/saltybot_panoramic/package.xml new file mode 100644 index 0000000..29eeee0 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_panoramic/package.xml @@ -0,0 +1,26 @@ + + + + saltybot_panoramic + 0.1.0 + 360° equirectangular panoramic stitching + RTSP stream from 4× IMX219 + seb + MIT + ament_cmake_python + ament_cmake + rclpy + sensor_msgs + cv_bridge + image_transport + std_msgs + python3-opencv + python3-numpy + python3-yaml + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + ament_cmake + + diff --git a/jetson/ros2_ws/src/saltybot_panoramic/resource/saltybot_panoramic b/jetson/ros2_ws/src/saltybot_panoramic/resource/saltybot_panoramic new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_panoramic/saltybot_panoramic/__init__.py b/jetson/ros2_ws/src/saltybot_panoramic/saltybot_panoramic/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_panoramic/saltybot_panoramic/calibration_loader.py b/jetson/ros2_ws/src/saltybot_panoramic/saltybot_panoramic/calibration_loader.py new file mode 100644 index 0000000..4430371 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_panoramic/saltybot_panoramic/calibration_loader.py @@ -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 diff --git a/jetson/ros2_ws/src/saltybot_panoramic/saltybot_panoramic/equirect_projector.py b/jetson/ros2_ws/src/saltybot_panoramic/saltybot_panoramic/equirect_projector.py new file mode 100644 index 0000000..cbb8f28 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_panoramic/saltybot_panoramic/equirect_projector.py @@ -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) diff --git a/jetson/ros2_ws/src/saltybot_panoramic/saltybot_panoramic/equirect_stitcher_node.py b/jetson/ros2_ws/src/saltybot_panoramic/saltybot_panoramic/equirect_stitcher_node.py new file mode 100644 index 0000000..047f2c7 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_panoramic/saltybot_panoramic/equirect_stitcher_node.py @@ -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() diff --git a/jetson/ros2_ws/src/saltybot_panoramic/saltybot_panoramic/panoramic_node.py b/jetson/ros2_ws/src/saltybot_panoramic/saltybot_panoramic/panoramic_node.py new file mode 100644 index 0000000..e3ee4ed --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_panoramic/saltybot_panoramic/panoramic_node.py @@ -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() diff --git a/jetson/ros2_ws/src/saltybot_panoramic/saltybot_panoramic/rtsp_server_node.py b/jetson/ros2_ws/src/saltybot_panoramic/saltybot_panoramic/rtsp_server_node.py new file mode 100644 index 0000000..47a36c3 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_panoramic/saltybot_panoramic/rtsp_server_node.py @@ -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() diff --git a/jetson/ros2_ws/src/saltybot_panoramic/scripts/build_rtsp_pipeline.sh b/jetson/ros2_ws/src/saltybot_panoramic/scripts/build_rtsp_pipeline.sh new file mode 100755 index 0000000..60ae6e1 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_panoramic/scripts/build_rtsp_pipeline.sh @@ -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 diff --git a/jetson/ros2_ws/src/saltybot_panoramic/scripts/test_stitcher.py b/jetson/ros2_ws/src/saltybot_panoramic/scripts/test_stitcher.py new file mode 100755 index 0000000..6f2498b --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_panoramic/scripts/test_stitcher.py @@ -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()) diff --git a/jetson/ros2_ws/src/saltybot_panoramic/setup.cfg b/jetson/ros2_ws/src/saltybot_panoramic/setup.cfg new file mode 100644 index 0000000..dcce8b0 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_panoramic/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/saltybot_panoramic +[install] +install_scripts=$base/lib/saltybot_panoramic diff --git a/jetson/ros2_ws/src/saltybot_panoramic/setup.py b/jetson/ros2_ws/src/saltybot_panoramic/setup.py new file mode 100644 index 0000000..075420f --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_panoramic/setup.py @@ -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', + ], + }, +)