jetson/config/nav2_params.yaml

@@ -255,7 +255,7 @@ local_costmap:
       obstacle_layer:
         plugin: "nav2_costmap_2d::ObstacleLayer"
         enabled: true
-        observation_sources: scan
+        observation_sources: scan surround_cameras
         scan:
           topic: /scan
           max_obstacle_height: 0.80
@@ -266,6 +266,18 @@ local_costmap:
           raytrace_min_range: 0.0
           obstacle_max_range: 7.5
           obstacle_min_range: 0.0
+        # 4× IMX219 camera-detected obstacles (chairs, glass, people)
+        surround_cameras:
+          topic: /surround_vision/obstacles
+          min_obstacle_height: 0.05
+          max_obstacle_height: 1.50
+          clearing: false
+          marking: true
+          data_type: "PointCloud2"
+          raytrace_max_range: 3.5
+          raytrace_min_range: 0.0
+          obstacle_max_range: 3.0
+          obstacle_min_range: 0.0
 
       voxel_layer:
         plugin: "nav2_costmap_2d::VoxelLayer"
@@ -317,7 +329,7 @@ global_costmap:
       obstacle_layer:
         plugin: "nav2_costmap_2d::ObstacleLayer"
         enabled: true
-        observation_sources: scan
+        observation_sources: scan surround_cameras
         scan:
           topic: /scan
           max_obstacle_height: 0.80
@@ -328,6 +340,17 @@ global_costmap:
           raytrace_min_range: 0.0
           obstacle_max_range: 7.5
           obstacle_min_range: 0.0
+        surround_cameras:
+          topic: /surround_vision/obstacles
+          min_obstacle_height: 0.05
+          max_obstacle_height: 1.50
+          clearing: false
+          marking: true
+          data_type: "PointCloud2"
+          raytrace_max_range: 3.5
+          raytrace_min_range: 0.0
+          obstacle_max_range: 3.0
+          obstacle_min_range: 0.0
 
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"

jetson/docker-compose.yml

@@ -164,6 +164,35 @@ services:
           fps:=30
       "
 
+  # ── Surround vision — 360° bird's-eye view + Nav2 camera obstacle layer ─────
+  saltybot-surround:
+    image: saltybot/ros2-humble:jetson-orin
+    build:
+      context: .
+      dockerfile: Dockerfile
+    container_name: saltybot-surround
+    restart: unless-stopped
+    runtime: nvidia
+    network_mode: host
+    depends_on:
+      - csi-cameras                 # IMX219 /camera/*/image_raw must be publishing
+    environment:
+      - NVIDIA_VISIBLE_DEVICES=all
+      - NVIDIA_DRIVER_CAPABILITIES=all
+      - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+      - ROS_DOMAIN_ID=42
+    volumes:
+      - ./ros2_ws/src:/ros2_ws/src:rw
+      - ./config:/config:ro
+    command: >
+      bash -c "
+        source /opt/ros/humble/setup.bash &&
+        ros2 launch saltybot_surround surround_vision.launch.py
+          start_cameras:=false
+          camera_height:=0.30
+          publish_rate:=5.0
+      "
+
   # ── Nav2 autonomous navigation stack ────────────────────────────────────────
   saltybot-nav2:
     image: saltybot/ros2-humble:jetson-orin

jetson/ros2_ws/src/saltybot_surround/config/surround_vision_params.yaml

@@ -0,0 +1,65 @@
+# surround_vision_params.yaml — 4× IMX219 surround vision pipeline
+#
+# Hardware: Jetson Orin Nano Super / 4× IMX219 160° CSI cameras
+# Camera mount: sensor head ring, r=5 cm, 90° intervals, ~0.30 m above floor
+#
+# ── IMX219 calibration note ─────────────────────────────────────────────────
+# Placeholder values are used for the fisheye model (k1=k2=k3=k4=0).
+# Run camera_calibration after hardware is assembled:
+#
+#   ros2 run camera_calibration cameracalibrator \
+#     --size 8x6 --square 0.025 \
+#     --camera-name camera_front \
+#     image:=/camera/front/image_raw \
+#     camera:=/camera/front
+#
+# Then save per-camera calibration to jetson/config/camera_{front,left,rear,right}_info.yaml
+# and set camera_info_url in saltybot_cameras/config/cameras_params.yaml.
+# ─────────────────────────────────────────────────────────────────────────────
+
+surround_costmap:
+  ros__parameters:
+    # Physical camera geometry
+    camera_height:       0.30     # metres above floor (measure after assembly)
+    camera_pitch_deg:    0.0      # positive = tilted down; adjust to match mount
+
+    # Processing resolution (downsampled from 640×480)
+    proc_width:          160
+    proc_height:         120
+
+    # Obstacle detection range
+    min_obstacle_range:  0.25     # metres from camera (ignore extremely close)
+    max_obstacle_range:  3.00     # metres from camera
+
+    # Canny edge thresholds (tune for lighting conditions)
+    canny_low:           40
+    canny_high:          120
+
+    # Z height of published PointCloud2 points (must be within Nav2 obstacle height band)
+    obstacle_pub_height: 0.50     # metres (Nav2 filters 0.05–1.5 m by default)
+
+    # Publish rate (Hz). Orin can handle 10 Hz; start at 5 to validate.
+    publish_rate:        5.0
+
+surround_vision:
+  ros__parameters:
+    # Physical camera geometry (must match surround_costmap)
+    camera_height:       0.30
+    camera_pitch_deg:    0.0
+
+    # IPM input resolution
+    ipm_input_width:     160
+    ipm_input_height:    120
+
+    # Bird's-eye patch size (pixels). One patch per camera direction.
+    ipm_patch_px:        80       # 80×80 px per camera → 160×160 composite
+
+    # Ground range shown per camera (metres from camera to far edge of patch)
+    ipm_range_m:         3.0
+
+    # Composite canvas size
+    canvas_m:            6.0      # metres per side
+    canvas_px:           240      # pixels per side → 0.025 m/pixel
+
+    # Publish rate for bird's-eye image
+    publish_rate:        10.0     # Hz

jetson/ros2_ws/src/saltybot_surround/launch/surround_vision.launch.py

@@ -0,0 +1,92 @@
+"""
+surround_vision.launch.py — 360° surround vision pipeline
+
+Launches:
+  1. saltybot_cameras/csi_cameras.launch.py  — IMX219 V4L2 camera drivers
+     (skip if cameras already running from a separate container)
+  2. surround_costmap_node  — obstacle PointCloud2 for Nav2
+  3. surround_vision_node   — 360° bird's-eye image for visualisation
+
+Arguments:
+  start_cameras  (default true)  — include csi_cameras.launch.py
+                                    set false if cameras run in separate process
+  camera_height  (default 0.30)  — sensor head height above floor (m)
+  camera_pitch   (default 0.0)   — camera tilt-down angle in degrees
+  max_range      (default 3.0)   — max obstacle detection range (m)
+  publish_rate   (default 5.0)   — costmap + vision publish frequency (Hz)
+"""
+
+import os
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    OpaqueFunction,
+)
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    bringup_dir = get_package_share_directory('saltybot_surround')
+    cameras_dir = get_package_share_directory('saltybot_cameras')
+
+    params_file = os.path.join(bringup_dir, 'config', 'surround_vision_params.yaml')
+
+    # ── Launch arguments ─────────────────────────────────────────────────────
+    args = [
+        DeclareLaunchArgument('start_cameras',  default_value='true'),
+        DeclareLaunchArgument('camera_height',  default_value='0.30'),
+        DeclareLaunchArgument('camera_pitch',   default_value='0.0'),
+        DeclareLaunchArgument('max_range',      default_value='3.0'),
+        DeclareLaunchArgument('publish_rate',   default_value='5.0'),
+    ]
+
+    # ── Camera drivers (optional — skip if already running) ──────────────────
+    csi_cameras = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(cameras_dir, 'launch', 'csi_cameras.launch.py')
+        ),
+        launch_arguments={'width': '640', 'height': '480', 'fps': '30'}.items(),
+        condition=IfCondition(LaunchConfiguration('start_cameras')),
+    )
+
+    # ── surround_costmap_node ─────────────────────────────────────────────────
+    costmap_node = Node(
+        package='saltybot_surround',
+        executable='surround_costmap_node',
+        name='surround_costmap',
+        output='screen',
+        parameters=[
+            params_file,
+            {
+                'camera_height':      LaunchConfiguration('camera_height'),
+                'camera_pitch_deg':   LaunchConfiguration('camera_pitch'),
+                'max_obstacle_range': LaunchConfiguration('max_range'),
+                'publish_rate':       LaunchConfiguration('publish_rate'),
+            },
+        ],
+    )
+
+    # ── surround_vision_node ──────────────────────────────────────────────────
+    vision_node = Node(
+        package='saltybot_surround',
+        executable='surround_vision_node',
+        name='surround_vision',
+        output='screen',
+        parameters=[
+            params_file,
+            {
+                'camera_height':   LaunchConfiguration('camera_height'),
+                'camera_pitch_deg': LaunchConfiguration('camera_pitch'),
+                'ipm_range_m':     LaunchConfiguration('max_range'),
+                'publish_rate':    LaunchConfiguration('publish_rate'),
+            },
+        ],
+    )
+
+    return LaunchDescription([*args, csi_cameras, costmap_node, vision_node])

jetson/ros2_ws/src/saltybot_surround/package.xml

@@ -0,0 +1,32 @@
+<?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_surround</name>
+  <version>0.1.0</version>
+  <description>
+    4× IMX219 surround vision pipeline — 360° bird's-eye view and Nav2
+    costmap obstacle layer from camera-based obstacle detection.
+    Catches obstacles that RPLIDAR misses (chairs, people, glass walls).
+  </description>
+  <maintainer email="sl-perception@saltylab.local">sl-perception</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>cv_bridge</depend>
+  <depend>image_transport</depend>
+  <depend>tf2_ros</depend>
+  <depend>geometry_msgs</depend>
+
+  <exec_depend>python3-opencv</exec_depend>
+  <exec_depend>python3-numpy</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_python</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_surround/saltybot_surround/surround_costmap_node.py

@@ -0,0 +1,315 @@
+"""
+surround_costmap_node — Camera-based obstacle detection for Nav2 costmap
+
+Subscribes to 4× IMX219 CSI camera feeds, projects potential obstacles onto
+the ground plane using a pinhole model + known camera geometry, and publishes
+them as a PointCloud2 on /surround_vision/obstacles.
+
+Nav2's obstacle_layer subscribes to this PointCloud2 as an observation source,
+supplementing RPLIDAR with obstacles that LIDAR misses:
+  - Transparent glass walls / doors
+  - Low furniture (chair legs, coffee tables)
+  - People detected by camera but below LIDAR scan plane
+
+Algorithm (V1):
+  1. Downsample each image to processing resolution (default 160×120)
+  2. Undistort with placeholder IMX219 fisheye calibration
+     (replace K, D with real calibration data once measured)
+  3. Detect edges (Canny) — edges indicate object boundaries
+  4. For each edge pixel below the horizon (v > cy), back-project to ground
+     using pinhole model + camera height:
+       distance = h_cam * fy / (row - cy)
+       lateral  = (col - cx) * distance / fx
+  5. Filter by range [min_obstacle_range, max_obstacle_range]
+  6. Rotate from camera frame to base_link using known camera yaw
+  7. Publish merged point cloud at obstacle height (z ≈ 0.5 m above ground)
+
+Camera yaws in base_link frame:
+  front=0°, left=90°, rear=180°, right=-90°
+
+Extension path (Phase 2c+):
+  Replace Canny with TensorRT person/obstacle detector for higher precision.
+  Use optical flow to distinguish moving obstacles from static background.
+"""
+
+import math
+import struct
+
+import cv2
+import numpy as np
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+
+from sensor_msgs.msg import Image, PointCloud2, PointField
+from cv_bridge import CvBridge
+
+
+# ── Camera geometry ──────────────────────────────────────────────────────────
+# Yaw angle (degrees) of each camera's optical axis in base_link frame.
+# 0° = facing +x (front), 90° = facing +y (left).
+_CAMERAS = [
+    ('front', 0.0),
+    ('left',  90.0),
+    ('rear',  180.0),
+    ('right', -90.0),
+]
+
+# IMX219 160° fisheye placeholder calibration (640×480 resolution).
+# fx/fy computed for equidistant fisheye: f = r_max / (FOV/2 in radians)
+# FOV=160°, r_max=320px → f = 320 / 1.396 ≈ 229 px
+# After undistortion to rectilinear, effective FOV ≈ 100°.
+# For the simplified pinhole model post-undistortion at 160×120:
+# fx = fy ≈ (80 / tan(50°)) ≈ 67 px
+_CAM_FX_FULL = 229.0
+_CAM_FY_FULL = 229.0
+_CAM_CX_FULL = 320.0
+_CAM_CY_FULL = 240.0
+# Fisheye distortion coefficients k1..k4 (cv2.fisheye model).
+# Zero means no correction beyond the equidistant model assumption.
+# Replace with real values from `ros2 run camera_calibration cameracalibrator`.
+_CAM_D = np.array([0.0, 0.0, 0.0, 0.0], dtype=np.float64)
+
+
+class SurroundCostmapNode(Node):
+    """Publish PointCloud2 of camera-detected obstacles for Nav2 costmap."""
+
+    def __init__(self):
+        super().__init__('surround_costmap')
+
+        # ── Parameters ──────────────────────────────────────────────────────
+        self.declare_parameter('camera_height',       0.30)   # m above ground
+        self.declare_parameter('camera_pitch_deg',    0.0)    # tilt down = positive
+        self.declare_parameter('proc_width',          160)    # processing resolution
+        self.declare_parameter('proc_height',         120)
+        self.declare_parameter('min_obstacle_range',  0.25)   # m from camera
+        self.declare_parameter('max_obstacle_range',  3.00)   # m from camera
+        self.declare_parameter('obstacle_pub_height', 0.50)   # z in PointCloud2 (m)
+        self.declare_parameter('canny_low',           40)
+        self.declare_parameter('canny_high',          120)
+        self.declare_parameter('publish_rate',        5.0)    # Hz
+
+        self._bridge = CvBridge()
+        self._latest: dict[str, Image] = {}
+
+        # ── Subscriptions ────────────────────────────────────────────────────
+        best_effort = QoSProfile(
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=1,
+        )
+        for name, _ in _CAMERAS:
+            self.create_subscription(
+                Image,
+                f'/camera/{name}/image_raw',
+                lambda msg, n=name: self._image_cb(n, msg),
+                best_effort,
+            )
+
+        # ── Publisher ────────────────────────────────────────────────────────
+        self._pub = self.create_publisher(PointCloud2, '/surround_vision/obstacles', 10)
+
+        # ── Timer ────────────────────────────────────────────────────────────
+        rate = self.get_parameter('publish_rate').value
+        self.create_timer(1.0 / rate, self._publish)
+
+        self.get_logger().info(
+            'surround_costmap: listening on /camera/{front,left,rear,right}/image_raw'
+        )
+
+    # ── Callbacks ────────────────────────────────────────────────────────────
+
+    def _image_cb(self, name: str, msg: Image) -> None:
+        self._latest[name] = msg
+
+    def _publish(self) -> None:
+        all_pts: list[tuple[float, float, float]] = []
+        stamp = self.get_clock().now().to_msg()
+
+        for cam_name, cam_yaw_deg in _CAMERAS:
+            if cam_name not in self._latest:
+                continue
+            try:
+                img_bgr = self._bridge.imgmsg_to_cv2(self._latest[cam_name], 'bgr8')
+            except Exception as exc:
+                self.get_logger().warn(f'{cam_name}: cv_bridge error: {exc}', throttle_duration_sec=5.0)
+                continue
+
+            pts = self._process_camera(img_bgr, cam_yaw_deg)
+            all_pts.extend(pts)
+
+        if not all_pts:
+            return
+
+        self._pub.publish(self._make_pc2(all_pts, 'base_link', stamp))
+
+    # ── Core processing ───────────────────────────────────────────────────────
+
+    def _process_camera(
+        self, img_bgr: np.ndarray, cam_yaw_deg: float
+    ) -> list[tuple[float, float, float]]:
+        """Detect edge-based obstacles and project to base_link ground coords."""
+
+        pw = self.get_parameter('proc_width').value
+        ph = self.get_parameter('proc_height').value
+        h_cam = self.get_parameter('camera_height').value
+        pitch_rad = math.radians(self.get_parameter('camera_pitch_deg').value)
+        d_min = self.get_parameter('min_obstacle_range').value
+        d_max = self.get_parameter('max_obstacle_range').value
+        obs_z = self.get_parameter('obstacle_pub_height').value
+        canny_lo = self.get_parameter('canny_low').value
+        canny_hi = self.get_parameter('canny_high').value
+
+        # ── Downsample & fisheye undistort ───────────────────────────────────
+        small = cv2.resize(img_bgr, (pw, ph), interpolation=cv2.INTER_AREA)
+        small = self._undistort(small)
+
+        # ── Effective intrinsics at processing resolution ────────────────────
+        scale_x = pw / 640.0
+        scale_y = ph / 480.0
+        fx = _CAM_FX_FULL * scale_x
+        fy = _CAM_FY_FULL * scale_y
+        cx = _CAM_CX_FULL * scale_x
+        cy = _CAM_CY_FULL * scale_y
+
+        # ── Obstacle detection via Canny edge detection ──────────────────────
+        gray = cv2.cvtColor(small, cv2.COLOR_BGR2GRAY)
+        # Equalise histogram for consistent thresholding across lighting conditions
+        gray = cv2.equalizeHist(gray)
+        edges = cv2.Canny(gray, canny_lo, canny_hi)
+
+        # Morphological close: connect nearby edge fragments
+        kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))
+        edges = cv2.morphologyEx(edges, cv2.MORPH_CLOSE, kernel)
+
+        # ── Ground projection ─────────────────────────────────────────────────
+        # Only pixels below the horizon (v > cy adjusted for pitch) can project
+        # onto the ground plane.
+        # Effective horizon row accounting for camera pitch:
+        #   pitch > 0 → camera tilts down → horizon shifts up (smaller v_horizon)
+        v_horizon = cy - fy * math.tan(pitch_rad)
+
+        rows, cols = np.where(edges > 0)
+        valid_mask = rows > v_horizon
+        rows = rows[valid_mask].astype(np.float64)
+        cols = cols[valid_mask].astype(np.float64)
+
+        if len(rows) == 0:
+            return []
+
+        # Back-project to camera frame (z forward, x right, y down)
+        # Camera at height h_cam, ground plane at y_cam = h_cam (y is down)
+        # For pitch angle φ (camera tilted down), adjust the horizon:
+        # Normalized ray direction: [x_n, y_n, 1]
+        x_n = (cols - cx) / fx
+        y_n = (rows - cy) / fy
+
+        # Rotate ray by camera pitch (about x-axis, positive = tilting down)
+        cos_p = math.cos(pitch_rad)
+        sin_p = math.sin(pitch_rad)
+        y_cam = y_n * cos_p + sin_p       # y component in camera frame
+        z_cam = -y_n * sin_p + cos_p      # z component in camera frame (forward)
+
+        # Ground intersection: camera y = h_cam → ray param t = h_cam / y_cam
+        valid_ground = y_cam > 0.01       # must be pointing toward floor
+        y_cam = y_cam[valid_ground]
+        z_cam = z_cam[valid_ground]
+        x_n_vg = x_n[valid_ground]
+
+        t = h_cam / y_cam
+        x_ground_cam = x_n_vg * t        # lateral in camera frame
+        z_ground_cam = z_cam * t          # forward in camera frame
+
+        # ── Range filter ─────────────────────────────────────────────────────
+        dist = np.hypot(x_ground_cam, z_ground_cam)
+        in_range = (dist >= d_min) & (dist <= d_max)
+        x_g = x_ground_cam[in_range]
+        z_g = z_ground_cam[in_range]
+
+        if len(x_g) == 0:
+            return []
+
+        # ── Rotate from camera frame to base_link ────────────────────────────
+        # Camera coordinate convention: z forward, x right.
+        # Robot base_link:              x forward, y left.
+        # Camera yaw θ = angle of camera +z axis from base_link +x axis.
+        #
+        # base_x =  cos(θ) * z_g  -  sin(θ) * x_g
+        # base_y =  sin(θ) * z_g  +  cos(θ) * x_g
+        # base_z =  obs_z  (obstacle height in base_link)
+        yaw = math.radians(cam_yaw_deg)
+        cos_y = math.cos(yaw)
+        sin_y = math.sin(yaw)
+        base_x = cos_y * z_g - sin_y * x_g
+        base_y = sin_y * z_g + cos_y * x_g
+
+        # Subsample to limit point cloud density (every 4th point)
+        step = max(1, len(base_x) // 200)
+        pts = [
+            (float(bx), float(by), obs_z)
+            for bx, by in zip(base_x[::step], base_y[::step])
+        ]
+        return pts
+
+    # ── Helpers ───────────────────────────────────────────────────────────────
+
+    def _undistort(self, img: np.ndarray) -> np.ndarray:
+        """Approximate fisheye undistortion with placeholder IMX219 calibration.
+
+        The fisheye calibration matrices are scaled to the processing resolution.
+        Replace _CAM_D with values from camera_calibration once measured.
+        """
+        h, w = img.shape[:2]
+        K = np.array([
+            [_CAM_FX_FULL * w / 640.0, 0.0, _CAM_CX_FULL * w / 640.0],
+            [0.0, _CAM_FY_FULL * h / 480.0, _CAM_CY_FULL * h / 480.0],
+            [0.0, 0.0, 1.0],
+        ], dtype=np.float64)
+        # cv2.fisheye.undistortImage handles equidistant fisheye model.
+        # With D=[0,0,0,0] this is a no-op (use as hook for real calibration).
+        if np.any(_CAM_D != 0.0):
+            return cv2.fisheye.undistortImage(img, K, _CAM_D, Knew=K)
+        return img
+
+    @staticmethod
+    def _make_pc2(
+        pts: list[tuple[float, float, float]],
+        frame_id: str,
+        stamp,
+    ) -> PointCloud2:
+        """Pack a list of (x, y, z) tuples into a PointCloud2 message."""
+        fields = [
+            PointField(name='x', offset=0,  datatype=PointField.FLOAT32, count=1),
+            PointField(name='y', offset=4,  datatype=PointField.FLOAT32, count=1),
+            PointField(name='z', offset=8,  datatype=PointField.FLOAT32, count=1),
+        ]
+        data = struct.pack(f'{3 * len(pts)}f', *[v for pt in pts for v in pt])
+        msg = PointCloud2()
+        msg.header.stamp = stamp
+        msg.header.frame_id = frame_id
+        msg.height = 1
+        msg.width = len(pts)
+        msg.fields = fields
+        msg.is_bigendian = False
+        msg.point_step = 12
+        msg.row_step = 12 * len(pts)
+        msg.is_dense = True
+        msg.data = data
+        return msg
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = SurroundCostmapNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_surround/saltybot_surround/surround_vision_node.py

@@ -0,0 +1,297 @@
+"""
+surround_vision_node — 360° bird's-eye panoramic view from 4× IMX219 cameras
+
+Each IMX219 image is warped to a top-down (bird's-eye) view using Inverse
+Perspective Mapping (IPM). The four patches are composited into a single
+overhead image centred on the robot, giving a 360° ground-level view.
+
+Published topics:
+  /surround_vision/birdseye  (sensor_msgs/Image, BGR8)
+    Square overhead image. Robot is at centre; North (robot +x) is up.
+    Scale: configurable, default 0.05 m/pixel → 200×200px = 10 m × 10 m.
+
+Algorithm:
+  1. Downsample each image to ipm_width × ipm_height
+  2. Compute IPM homography from camera geometry:
+       - Source: 4 image points corresponding to known ground distances
+       - Destination: corresponding points in bird's-eye canvas
+  3. cv2.warpPerspective to produce top-down patch (ipm_patch_size² pixels)
+  4. Place each patch in the correct direction on the composite canvas:
+       - Front patch → above robot centre
+       - Rear patch  → below robot centre
+       - Left patch  → left of robot centre
+       - Right patch → right of robot centre
+  5. Draw robot footprint circle at canvas centre
+  6. Publish composite image
+
+Notes:
+  - Camera intrinsics are scaled placeholders.  Replace once calibrated.
+  - For best results, mount cameras with a slight downward tilt (5-10°).
+    Adjust camera_pitch_deg parameter to match hardware.
+  - The IPM assumes a flat floor.  Bumps and ramps will cause distortion.
+"""
+
+import math
+
+import cv2
+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
+
+
+_CAMERAS = [
+    # (name,  yaw_deg,  placement_fn_name)
+    ('front', 0.0,    'top'),
+    ('left',  90.0,   'left'),
+    ('rear',  180.0,  'bottom'),
+    ('right', -90.0,  'right'),
+]
+
+_CAM_FX_FULL = 229.0
+_CAM_FY_FULL = 229.0
+_CAM_CX_FULL = 320.0
+_CAM_CY_FULL = 240.0
+
+
+class SurroundVisionNode(Node):
+    """Composite 4× IMX219 bird's-eye patches into a 360° overhead image."""
+
+    def __init__(self):
+        super().__init__('surround_vision')
+
+        # ── Parameters ──────────────────────────────────────────────────────
+        self.declare_parameter('camera_height',   0.30)   # m
+        self.declare_parameter('camera_pitch_deg', 0.0)   # tilt down = positive
+        self.declare_parameter('ipm_input_width',  160)
+        self.declare_parameter('ipm_input_height', 120)
+        self.declare_parameter('ipm_patch_px',     80)    # patch size (pixels)
+        self.declare_parameter('ipm_range_m',      2.0)   # depth range shown (m)
+        self.declare_parameter('canvas_m',         6.0)   # canvas side (m)
+        self.declare_parameter('canvas_px',        240)   # canvas pixels per side
+        self.declare_parameter('publish_rate',     10.0)  # Hz
+
+        self._bridge = CvBridge()
+        self._latest: dict[str, np.ndarray] = {}
+        self._H: dict[str, np.ndarray | None] = {n: None for n, *_ in _CAMERAS}
+
+        # ── Subscriptions ────────────────────────────────────────────────────
+        best_effort = QoSProfile(
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=1,
+        )
+        for name, *_ in _CAMERAS:
+            self.create_subscription(
+                Image,
+                f'/camera/{name}/image_raw',
+                lambda msg, n=name: self._image_cb(n, msg),
+                best_effort,
+            )
+
+        # ── Publisher ────────────────────────────────────────────────────────
+        self._pub = self.create_publisher(Image, '/surround_vision/birdseye', 10)
+
+        rate = self.get_parameter('publish_rate').value
+        self.create_timer(1.0 / rate, self._publish)
+
+        self.get_logger().info(
+            'surround_vision: compositing 4× IMX219 → /surround_vision/birdseye'
+        )
+
+    # ── Callbacks ─────────────────────────────────────────────────────────────
+
+    def _image_cb(self, name: str, msg: Image) -> None:
+        try:
+            self._latest[name] = self._bridge.imgmsg_to_cv2(msg, 'bgr8')
+        except Exception:
+            pass
+
+    def _publish(self) -> None:
+        canvas_px = self.get_parameter('canvas_px').value
+        canvas = np.zeros((canvas_px, canvas_px, 3), dtype=np.uint8)
+
+        cx_px = canvas_px // 2   # robot centre in canvas
+        cy_px = canvas_px // 2
+
+        for cam_name, cam_yaw_deg, placement in _CAMERAS:
+            if cam_name not in self._latest:
+                continue
+            patch = self._make_birdseye_patch(cam_name, self._latest[cam_name])
+            if patch is None:
+                continue
+            self._place_patch(canvas, patch, placement, cx_px, cy_px)
+
+        # Draw robot footprint
+        canvas_m = self.get_parameter('canvas_m').value
+        px_per_m = canvas_px / canvas_m
+        robot_radius_px = max(2, int(0.15 * px_per_m))   # robot_radius = 0.15 m
+        cv2.circle(canvas, (cx_px, cy_px), robot_radius_px, (0, 255, 0), 1)
+        cv2.drawMarker(canvas, (cx_px, cy_px), (0, 255, 0),
+                       cv2.MARKER_CROSS, markerSize=6, thickness=1)
+
+        # Draw compass label
+        cv2.putText(canvas, 'N', (cx_px - 5, 10),
+                    cv2.FONT_HERSHEY_SIMPLEX, 0.4, (200, 200, 200), 1)
+
+        msg = self._bridge.cv2_to_imgmsg(canvas, 'bgr8')
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = 'base_link'
+        self._pub.publish(msg)
+
+    # ── IPM ───────────────────────────────────────────────────────────────────
+
+    def _make_birdseye_patch(self, cam_name: str, img_bgr: np.ndarray) -> np.ndarray | None:
+        """Warp one camera image to a top-down bird's-eye patch."""
+        iw = self.get_parameter('ipm_input_width').value
+        ih = self.get_parameter('ipm_input_height').value
+        patch_px = self.get_parameter('ipm_patch_px').value
+        h_cam = self.get_parameter('camera_height').value
+        pitch_rad = math.radians(self.get_parameter('camera_pitch_deg').value)
+        range_m = self.get_parameter('ipm_range_m').value
+
+        small = cv2.resize(img_bgr, (iw, ih), interpolation=cv2.INTER_AREA)
+
+        # Compute / cache homography for this camera at current params
+        if self._H[cam_name] is None:
+            H = self._compute_ipm_H(iw, ih, h_cam, pitch_rad, range_m, patch_px)
+            self._H[cam_name] = H
+
+        if self._H[cam_name] is None:
+            return None
+
+        return cv2.warpPerspective(small, self._H[cam_name], (patch_px, patch_px))
+
+    def _compute_ipm_H(
+        self, iw: int, ih: int,
+        h_cam: float, pitch_rad: float,
+        range_m: float, patch_px: int,
+    ) -> np.ndarray | None:
+        """Compute IPM homography: image plane → bird's-eye top-down patch.
+
+        The IPM maps 4 ground control points, defined by their image coordinates
+        (computed from the pinhole model) to their positions in the output patch.
+
+        Output patch layout (for front camera):
+          - Top row    = farthest ground (range_m ahead of camera)
+          - Bottom row = nearest ground (0.5 m ahead of camera)
+          - Left/right = lateral extent ±(range_m/2) m
+        """
+        fx = _CAM_FX_FULL * iw / 640.0
+        fy = _CAM_FY_FULL * ih / 480.0
+        cx = _CAM_CX_FULL * iw / 640.0
+        cy = _CAM_CY_FULL * ih / 480.0
+
+        d_near = max(0.3, h_cam * 0.5)  # nearest ground distance (avoid singularity)
+        d_far = range_m
+        lat = range_m / 2.0
+
+        # Image y-coordinate of a ground point at forward distance d:
+        # Camera points slightly down by pitch_rad.
+        # After pitch rotation, the ground point at distance d is:
+        #   y_cam_norm = (h_cam / d - sin(pitch_rad)) / cos(pitch_rad)
+        # → v = cy + fy * y_cam_norm
+        def _row(d):
+            y_n = (h_cam / d - math.sin(pitch_rad)) / math.cos(pitch_rad)
+            return cy + fy * y_n
+
+        def _col(d, lat_m):
+            return cx + fx * lat_m / d
+
+        v_near = _row(d_near)
+        v_far = _row(d_far)
+        u_lnear = _col(d_near, -lat)
+        u_rnear = _col(d_near,  lat)
+        u_lfar  = _col(d_far,  -lat)
+        u_rfar  = _col(d_far,   lat)
+
+        # Sanity: far points must be above near points in image
+        if v_far >= v_near:
+            self.get_logger().warn('IPM: far row >= near row; check camera_height/pitch')
+            return None
+
+        src = np.float32([
+            [u_rfar,  v_far],   # far right
+            [u_lfar,  v_far],   # far left
+            [u_rnear, v_near],  # near right
+            [u_lnear, v_near],  # near left
+        ])
+        # In output patch: near = bottom, far = top; right = right, left = left
+        p = patch_px - 1
+        dst = np.float32([
+            [p,   0],   # far right  → top-right
+            [0,   0],   # far left   → top-left
+            [p,   p],   # near right → bottom-right
+            [0,   p],   # near left  → bottom-left
+        ])
+        return cv2.getPerspectiveTransform(src, dst)
+
+    # ── Canvas placement ──────────────────────────────────────────────────────
+
+    def _place_patch(
+        self,
+        canvas: np.ndarray,
+        patch: np.ndarray,
+        placement: str,
+        cx_px: int,
+        cy_px: int,
+    ) -> None:
+        """Place a bird's-eye patch on the composite canvas.
+
+        Placement rules (patch near edge = close to robot):
+          'top'    → front camera: patch bottom touches robot centre (y grows down)
+          'bottom' → rear camera:  patch top touches robot centre
+          'left'   → left camera:  patch right edge touches robot centre
+          'right'  → right camera: patch left edge touches robot centre
+        """
+        ph, pw = patch.shape[:2]
+        canvas_h, canvas_w = canvas.shape[:2]
+
+        if placement == 'top':       # front: near-ground at bottom of patch → cy_px
+            x0, y0 = cx_px - pw // 2, cy_px - ph
+        elif placement == 'bottom':  # rear: near-ground at top of patch → cy_px
+            # Rotate patch 180° so near-ground faces the robot
+            patch = cv2.rotate(patch, cv2.ROTATE_180)
+            x0, y0 = cx_px - pw // 2, cy_px
+        elif placement == 'left':    # left: near-ground at right edge → cx_px
+            # Rotate patch 90° CW so near-ground faces right (toward robot)
+            patch = cv2.rotate(patch, cv2.ROTATE_90_CLOCKWISE)
+            ph, pw = patch.shape[:2]
+            x0, y0 = cx_px - pw, cy_px - ph // 2
+        elif placement == 'right':   # right: near-ground at left edge → cx_px
+            # Rotate patch 90° CCW so near-ground faces left (toward robot)
+            patch = cv2.rotate(patch, cv2.ROTATE_90_COUNTERCLOCKWISE)
+            ph, pw = patch.shape[:2]
+            x0, y0 = cx_px, cy_px - ph // 2
+        else:
+            return
+
+        # Clip to canvas bounds
+        x1, y1 = x0 + pw, y0 + ph
+        px0 = max(0, -x0);     py0 = max(0, -y0)
+        cx0 = max(0,  x0);     cy0 = max(0,  y0)
+        cx1 = min(canvas_w, x1); cy1 = min(canvas_h, y1)
+        if cx1 <= cx0 or cy1 <= cy0:
+            return
+
+        canvas[cy0:cy1, cx0:cx1] = patch[py0:py0 + (cy1 - cy0), px0:px0 + (cx1 - cx0)]
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = SurroundVisionNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_surround/setup.cfg

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

jetson/ros2_ws/src/saltybot_surround/setup.py

@@ -0,0 +1,33 @@
+from setuptools import setup
+import os
+from glob import glob
+
+package_name = 'saltybot_surround'
+
+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='sl-perception',
+    maintainer_email='sl-perception@saltylab.local',
+    description='4x IMX219 surround vision pipeline and Nav2 costmap obstacle layer',
+    license='MIT',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'surround_costmap_node = saltybot_surround.surround_costmap_node:main',
+            'surround_vision_node  = saltybot_surround.surround_vision_node:main',
+        ],
+    },
+)

projects/saltybot/SLAM-SETUP-PLAN.md

@@ -91,8 +91,8 @@ Jetson Orin Nano Super (Ubuntu 22.04 / JetPack 6 / CUDA 12.x)
 |-------|--------|-------------|
 | 2a | ✅ Done (PR #17) | Sensor drivers — `saltybot_bringup` package |
 | 2a+ | ✅ Done (PR #36) | Orin update: Dockerfile JetPack 6, RTAB-Map launch + config |
-| 2b | ✅ Done (this PR) | Nav2 integration — path planning + obstacle avoidance |
-| 2c | Pending | 4× IMX219 surround vision (new bead, after hardware arrives) |
+| 2b | ✅ Done (PR #49) | Nav2 integration — path planning + obstacle avoidance |
+| 2c | ✅ Done (this PR) | 4× IMX219 surround vision + Nav2 camera obstacle layer |
 
 ---