From dc01efe32363c451e57bc5c31491df40724188ed Mon Sep 17 00:00:00 2001 From: sl-perception Date: Sat, 28 Feb 2026 23:19:23 -0500 Subject: [PATCH] feat: 4x IMX219 surround vision + Nav2 camera obstacle layer (Phase 2c) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit New ROS2 package saltybot_surround: surround_costmap_node - Subscribes to /camera/{front,left,rear,right}/image_raw - Detects obstacles via Canny edge detection + ground projection - Pinhole back-projection: pixel row → forward distance (d = h*fy/(v-cy)) - Rotates per-camera points to base_link frame using known camera yaws - Publishes /surround_vision/obstacles (PointCloud2, 5 Hz) - Catches chairs, glass walls, people that RPLIDAR misses - Placeholder IMX219 fisheye calibration (hook for real cal via cv2.fisheye) surround_vision_node - IPM homography computed from camera height + pinhole model - 4× bird's-eye patches composited into 240×240px 360° overhead view - Publishes /surround_vision/birdseye (Image, 10 Hz) - Robot footprint + compass overlay surround_vision.launch.py - Launches both nodes with surround_vision_params.yaml - start_cameras arg: set false when csi-cameras container runs separately Updated: - jetson/config/nav2_params.yaml add surround_cameras PointCloud2 source to local + global costmap obstacle_layer - jetson/docker-compose.yml add saltybot-surround service (depends_on: csi-cameras, start_cameras:=false) - projects/saltybot/SLAM-SETUP-PLAN.md Phase 2c ✅ Done Calibration TODO (run after hardware assembly): ros2 run camera_calibration cameracalibrator --size 8x6 --square 0.025 \ image:=/camera/front/image_raw camera:=/camera/front Replace placeholder K/D in surround_costmap_node._undistort() Co-Authored-By: Claude Sonnet 4.6 --- jetson/config/nav2_params.yaml | 27 +- jetson/docker-compose.yml | 29 ++ .../config/surround_vision_params.yaml | 65 ++++ .../launch/surround_vision.launch.py | 92 +++++ .../ros2_ws/src/saltybot_surround/package.xml | 32 ++ .../resource/saltybot_surround | 0 .../saltybot_surround/__init__.py | 0 .../surround_costmap_node.py | 315 ++++++++++++++++++ .../saltybot_surround/surround_vision_node.py | 297 +++++++++++++++++ .../ros2_ws/src/saltybot_surround/setup.cfg | 4 + jetson/ros2_ws/src/saltybot_surround/setup.py | 33 ++ projects/saltybot/SLAM-SETUP-PLAN.md | 4 +- 12 files changed, 894 insertions(+), 4 deletions(-) create mode 100644 jetson/ros2_ws/src/saltybot_surround/config/surround_vision_params.yaml create mode 100644 jetson/ros2_ws/src/saltybot_surround/launch/surround_vision.launch.py create mode 100644 jetson/ros2_ws/src/saltybot_surround/package.xml create mode 100644 jetson/ros2_ws/src/saltybot_surround/resource/saltybot_surround create mode 100644 jetson/ros2_ws/src/saltybot_surround/saltybot_surround/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_surround/saltybot_surround/surround_costmap_node.py create mode 100644 jetson/ros2_ws/src/saltybot_surround/saltybot_surround/surround_vision_node.py create mode 100644 jetson/ros2_ws/src/saltybot_surround/setup.cfg create mode 100644 jetson/ros2_ws/src/saltybot_surround/setup.py diff --git a/jetson/config/nav2_params.yaml b/jetson/config/nav2_params.yaml index dfda899..dd774d3 100644 --- a/jetson/config/nav2_params.yaml +++ b/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" diff --git a/jetson/docker-compose.yml b/jetson/docker-compose.yml index 93ef72e..eec51e4 100644 --- a/jetson/docker-compose.yml +++ b/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 diff --git a/jetson/ros2_ws/src/saltybot_surround/config/surround_vision_params.yaml b/jetson/ros2_ws/src/saltybot_surround/config/surround_vision_params.yaml new file mode 100644 index 0000000..157dd86 --- /dev/null +++ b/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 diff --git a/jetson/ros2_ws/src/saltybot_surround/launch/surround_vision.launch.py b/jetson/ros2_ws/src/saltybot_surround/launch/surround_vision.launch.py new file mode 100644 index 0000000..27ce671 --- /dev/null +++ b/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]) diff --git a/jetson/ros2_ws/src/saltybot_surround/package.xml b/jetson/ros2_ws/src/saltybot_surround/package.xml new file mode 100644 index 0000000..170690e --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_surround/package.xml @@ -0,0 +1,32 @@ + + + + saltybot_surround + 0.1.0 + + 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). + + sl-perception + MIT + + rclpy + sensor_msgs + cv_bridge + image_transport + tf2_ros + geometry_msgs + + python3-opencv + python3-numpy + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_surround/resource/saltybot_surround b/jetson/ros2_ws/src/saltybot_surround/resource/saltybot_surround new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_surround/saltybot_surround/__init__.py b/jetson/ros2_ws/src/saltybot_surround/saltybot_surround/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_surround/saltybot_surround/surround_costmap_node.py b/jetson/ros2_ws/src/saltybot_surround/saltybot_surround/surround_costmap_node.py new file mode 100644 index 0000000..7238cd6 --- /dev/null +++ b/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() diff --git a/jetson/ros2_ws/src/saltybot_surround/saltybot_surround/surround_vision_node.py b/jetson/ros2_ws/src/saltybot_surround/saltybot_surround/surround_vision_node.py new file mode 100644 index 0000000..9866d6e --- /dev/null +++ b/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() diff --git a/jetson/ros2_ws/src/saltybot_surround/setup.cfg b/jetson/ros2_ws/src/saltybot_surround/setup.cfg new file mode 100644 index 0000000..954dd1c --- /dev/null +++ b/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 diff --git a/jetson/ros2_ws/src/saltybot_surround/setup.py b/jetson/ros2_ws/src/saltybot_surround/setup.py new file mode 100644 index 0000000..26a3943 --- /dev/null +++ b/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', + ], + }, +) diff --git a/projects/saltybot/SLAM-SETUP-PLAN.md b/projects/saltybot/SLAM-SETUP-PLAN.md index 3a0d7b0..4d7e507 100644 --- a/projects/saltybot/SLAM-SETUP-PLAN.md +++ b/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 | --- -- 2.47.2