# Depth Camera Node

RGB-D camera interface for 3D vision sensors that capture color and depth data.

## Quick Start

```rust
use horus_library::nodes::{DepthCameraNode, CameraModel};
use horus_library::{DepthImage, PointCloud};
use horus_core::{Scheduler, Hub};

fn main() -> Result<(), Box<dyn std::error::Error>> {
    let mut scheduler = Scheduler::new();

    // Option 1: Simple setup with camera model preset
    let camera = DepthCameraNode::new(CameraModel::RealSenseD435)?;

    // Option 2: Builder pattern with custom configuration
    let camera = DepthCameraNode::builder()
        .model(CameraModel::RealSenseD435)
        .resolution(640, 480)
        .frame_rate(30)
        .enable_pointcloud(true)
        .build()?;

    scheduler.add(Box::new(camera), 50, Some(true));
    scheduler.run()?;
    Ok(())
}

// Subscribe to depth data in another node:
let depth_hub = Hub::<DepthImage>::new("depth_camera.depth.image")?;
if let Some(depth) = depth_hub.recv_latest() {
    println!("Depth image: {}x{}", depth.width, depth.height);
}

// Subscribe to point cloud:
let pc_hub = Hub::<PointCloud>::new("depth_camera.pointcloud")?;
if let Some(pc) = pc_hub.recv_latest() {
    println!("Point cloud: {} points", pc.points.len());
}
```

**Publishes to:** `depth_camera.rgb.image`, `depth_camera.depth.image`, `depth_camera.pointcloud`, `depth_camera.camera_info`

**Requires:** Enable camera feature in Cargo.toml: `features = ["realsense"]` or `features = ["zed"]`

## Overview

The Depth Camera Node provides a unified interface for depth-sensing cameras used in robotics for obstacle detection, 3D mapping, SLAM, and manipulation tasks. It publishes RGB images, depth images, point clouds, and camera calibration data.

### Supported Cameras

| Manufacturer | Models | Technology | Range |
|-------------|--------|------------|-------|
| **Intel RealSense** | D415, D435, D435i, D455, L515 | Active Stereo / LiDAR | 0.3-10m |
| **Stereolabs** | ZED, ZED 2, ZED 2i, ZED Mini | Passive Stereo | 0.5-20m |
| **Microsoft** | Kinect v1, Kinect v2, Azure Kinect | Structured Light / ToF | 0.5-5m |
| **Orbbec** | Astra, Astra Pro, Femto | Structured Light / ToF | 0.5-8m |
| **Luxonis** | OAK-D, OAK-D Lite, OAK-D Pro | Stereo + Neural | 0.2-35m |

### Key Features

- RGB color stream (up to 1920x1080 @ 30fps)
- Depth stream (up to 1280x720 @ 90fps)
- Point cloud generation
- Hardware depth-to-color alignment
- Post-processing filters (spatial, temporal, hole-filling)
- IMU data support (D435i, ZED2i, Azure Kinect)
- Simulation mode for testing without hardware

## Topics

### Publishers

| Topic | Type | Description |
|-------|------|-------------|
| `depth_camera.rgb.image` | `Image` | RGB color image |
| `depth_camera.depth.image` | `DepthImage` | Depth image (mm or m) |
| `depth_camera.pointcloud` | `PointCloud` | 3D point cloud |
| `depth_camera.camera_info` | `CameraInfo` | Camera intrinsics and calibration |

## Configuration Parameters

| Parameter | Type | Default | Description |
|-----------|------|---------|-------------|
| `camera_model` | `CameraModel` | Required | Camera model (e.g., `RealSenseD435`) |
| `resolution` | `(u32, u32)` | `(640, 480)` | RGB resolution (width, height) |
| `depth_resolution` | `(u32, u32)` | `(640, 480)` | Depth resolution |
| `frame_rate` | `u32` | `30` | Frame rate in Hz |
| `depth_range` | `(f32, f32)` | `(0.3, 10.0)` | Min/max depth in meters |
| `enable_pointcloud` | `bool` | `false` | Enable point cloud generation |
| `align_depth_to_color` | `bool` | `true` | Align depth to RGB frame |

## Message Types

### DepthImage

Depth image with per-pixel distance values:

```rust
pub struct DepthImage {
    pub width: u32,
    pub height: u32,
    pub encoding: u8,        // Depth encoding (16UC1, 32FC1)
    pub data: Vec<u8>,       // Raw depth data
    pub min_depth: f32,      // Minimum valid depth (meters)
    pub max_depth: f32,      // Maximum valid depth (meters)
    pub timestamp: u64,
}
```

### PointCloud

3D point cloud with optional color:

```rust
pub struct PointCloud {
    pub width: u32,
    pub height: u32,
    pub points: Vec<Point3D>,  // XYZ positions
    pub colors: Vec<[u8; 3]>,  // RGB colors (optional)
    pub timestamp: u64,
}
```

## Public API

### Construction

```rust
use horus_library::nodes::{DepthCameraNode, CameraModel};

// Create with specific camera model
let mut camera = DepthCameraNode::new(CameraModel::RealSenseD435)?;

// Use builder for advanced configuration
let camera = DepthCameraNode::builder()
    .model(CameraModel::RealSenseD435)
    .resolution(1280, 720)
    .frame_rate(30)
    .enable_pointcloud(true)
    .build()?;
```

### Configuration Methods

```rust
// Set resolution
camera.set_resolution(1280, 720);
camera.set_depth_resolution(640, 480);

// Set frame rate
camera.set_frame_rate(30);

// Set depth range (meters)
camera.set_depth_range(0.3, 10.0);

// Enable features
camera.enable_pointcloud(true);
camera.enable_depth_alignment(true);

// Post-processing filters
camera.enable_spatial_filter(true);
camera.enable_temporal_filter(true);
camera.enable_hole_filling(true);
```

## Usage Examples

### Basic Depth Sensing

```rust
use horus_library::prelude::*;
use horus_library::nodes::{DepthCameraNode, CameraModel};

fn main() -> Result<(), Box<dyn std::error::Error>> {
    let mut scheduler = Scheduler::new();

    // Create depth camera node
    let mut camera = DepthCameraNode::new(CameraModel::RealSenseD435)?;
    camera.set_resolution(640, 480);
    camera.set_frame_rate(30);

    scheduler.add(Box::new(camera), 30, Some(true));
    scheduler.run()?;
    Ok(())
}
```

### Obstacle Detection

```rust
use horus_library::prelude::*;
use horus_library::nodes::{DepthCameraNode, CameraModel};
use horus_library::DepthImage;

struct ObstacleDetector {
    depth_sub: Hub<DepthImage>,
    min_distance: f32,
}

impl ObstacleDetector {
    fn new(min_distance: f32) -> Result<Self, Box<dyn std::error::Error>> {
        Ok(Self {
            depth_sub: Hub::new("depth_camera.depth.image")?,
            min_distance,
        })
    }
}

impl Node for ObstacleDetector {
    fn name(&self) -> &'static str { "ObstacleDetector" }

    fn tick(&mut self, mut ctx: Option<&mut NodeInfo>) {
        if let Some(depth) = self.depth_sub.recv(&mut ctx) {
            // Check center region for obstacles
            let center_x = depth.width / 2;
            let center_y = depth.height / 2;

            // Get depth at center (implementation depends on encoding)
            let center_depth = get_depth_at(&depth, center_x, center_y);

            if center_depth < self.min_distance && center_depth > 0.0 {
                if let Some(ctx) = ctx.as_mut() {
                    ctx.log_warning(&format!(
                        "Obstacle detected at {:.2}m!", center_depth
                    ));
                }
            }
        }
    }
}

fn main() -> Result<(), Box<dyn std::error::Error>> {
    let mut scheduler = Scheduler::new();

    let mut camera = DepthCameraNode::new(CameraModel::RealSenseD435)?;
    camera.set_resolution(640, 480);

    let detector = ObstacleDetector::new(0.5)?;  // 0.5m minimum distance

    scheduler.add(Box::new(camera), 30, Some(true));
    scheduler.add(Box::new(detector), 30, Some(true));
    scheduler.run()?;
    Ok(())
}
```

### Point Cloud for 3D Mapping

```rust
use horus_library::prelude::*;
use horus_library::nodes::{DepthCameraNode, CameraModel};
use horus_library::PointCloud;

fn main() -> Result<(), Box<dyn std::error::Error>> {
    let mut scheduler = Scheduler::new();

    // Enable point cloud generation
    let camera = DepthCameraNode::builder()
        .model(CameraModel::RealSenseD435)
        .resolution(640, 480)
        .frame_rate(15)  // Lower rate for point cloud processing
        .enable_pointcloud(true)
        .build()?;

    scheduler.add(Box::new(camera), 15, Some(true));
    scheduler.run()?;
    Ok(())
}
```

### Hybrid Pattern with Processing

```rust
use horus_library::prelude::*;
use horus_library::nodes::{DepthCameraNode, CameraModel};
use horus_library::DepthImage;

fn main() -> Result<(), Box<dyn std::error::Error>> {
    let mut scheduler = Scheduler::new();

    // Apply custom processing to depth images
    let camera = DepthCameraNode::builder()
        .model(CameraModel::RealSenseD435)
        .with_closure(|mut depth: DepthImage| {
            // Filter out far distances
            depth.max_depth = 5.0;  // Limit to 5 meters
            depth
        })
        .build()?;

    scheduler.add(Box::new(camera), 30, Some(true));
    scheduler.run()?;
    Ok(())
}
```

## Camera Model Presets

### Intel RealSense D435

```rust
// Best for: Indoor navigation, manipulation
let mut camera = DepthCameraNode::new(CameraModel::RealSenseD435)?;
// Defaults: 1280x720 @ 30fps, 0.3-10m range
```

### Intel RealSense L515

```rust
// Best for: High-precision scanning, small object detection
let mut camera = DepthCameraNode::new(CameraModel::RealSenseL515)?;
// Defaults: 1024x768 @ 30fps, LiDAR-based, ±5mm accuracy
```

### Azure Kinect

```rust
// Best for: Body tracking, large FOV applications
let mut camera = DepthCameraNode::new(CameraModel::AzureKinect)?;
// Defaults: 1024x1024 @ 30fps, ToF, 0.25-5.46m range
```

## Hardware Setup

### Intel RealSense

```bash
# Install librealsense2
sudo apt install librealsense2-dkms librealsense2-utils

# Verify camera connection
realsense-viewer

# Check USB permissions
sudo usermod -a -G video $USER
```

### Enable in Project

Add to `Cargo.toml` or `horus.yaml`:

```toml
[dependencies]
horus_library = { version = "0.1", features = ["realsense"] }
```

Or in `horus.yaml`:
```yaml
features:
  - realsense
```

## Troubleshooting

### Issue: "Hardware unavailable - using SIMULATION mode"

**Solutions:**
1. Check camera is connected: `lsusb | grep -i intel`
2. Install drivers: `sudo apt install librealsense2-dkms`
3. Check USB permissions: `sudo usermod -a -G video $USER`
4. Rebuild with feature: `cargo build --features realsense`

### Issue: Low frame rate

**Cause:** High resolution or point cloud generation

**Solution:**
```rust
// Reduce resolution
camera.set_resolution(640, 480);

// Disable point cloud if not needed
camera.enable_pointcloud(false);

// Use depth-only mode
camera.enable_rgb(false);
```

### Issue: Noisy depth data

**Solution:**
```rust
// Enable post-processing filters
camera.enable_spatial_filter(true);
camera.enable_temporal_filter(true);
camera.enable_hole_filling(true);
```

## Simulation Mode

When hardware is unavailable, the node operates in simulation mode:

- Generates synthetic depth patterns
- Useful for algorithm development and testing
- No hardware dependencies required

```rust
// Force simulation mode
let mut camera = DepthCameraNode::new_with_backend(
    CameraModel::RealSenseD435,
    DepthBackend::Simulation
)?;
```

## Performance Considerations

| Resolution | Frame Rate | CPU Usage | Use Case |
|------------|------------|-----------|----------|
| 640x480 | 30fps | Low | Navigation, obstacle avoidance |
| 1280x720 | 30fps | Medium | Mapping, manipulation |
| 1280x720 | 60fps | High | High-speed tracking |
| + Point Cloud | -50% fps | High | 3D reconstruction |

## Related Nodes

- **CameraNode**: RGB-only camera for visual processing
- **LidarNode**: 2D/3D laser scanning
- **LocalizationNode**: Uses depth for SLAM
- **CollisionDetectorNode**: Obstacle detection from depth

## See Also

- [Intel RealSense SDK](https://github.com/IntelRealSense/librealsense)
- [Point Cloud Library](https://pointclouds.org/)
- [Open3D](http://www.open3d.org/)
