Camera Node (cam_node.py)

flow Diagram

graph TD
    A[Start] --> B{Initialize ROS 2};
    B --> C[Create cam_node];
    C --> D[Declare Parameters];
    D --> E[Get Parameter Values];
    E --> F[Create Image Publisher];
    F --> G[Create Camera Feed Thread];
    G --> H[Start Camera Feed Thread];
    H --> I[feed function starts];
    I --> J[Open Camera Device];
    J --> K[Set Camera Properties];
    K --> L{Camera Opened?};
    L -- Yes --> M[Start Timer for FPS];
    M --> N[while True Loop];
    N --> O[Read Frame from Camera];
    O --> P{Frame Read Successful?};
    P -- No --> N;
    P -- Yes --> Q[Convert BGR to RGB];
    Q --> R[Create ROS Image Message];
    R --> S[Set Message Header and Frame ID];
    S --> T{Resize Image?};
    T -- Yes --> U[Resize Image];
    U --> V[Set Message Dimensions to Resized];
    V --> W[Set Message Encoding rgb8];
    W --> X[Convert Image to Bytes];
    X --> Y[Populate Message Data];
    Y --> Z[Publish Image Message];
    Z --> AA[Calculate and Print FPS];
    AA --> AB[Reset Timer];
    AB --> N;
    T -- No --> AC[Set Message Dimensions to Original];
    AC --> W;
    H --> AD[rclpy.spin node];
    AD --> AE[Handle Keyboard Interrupt];
    AE --> AF[Raise KeyboardInterrupt];
    AD --> AG[Node Shutdown];
    AG --> AH[ROS 2 Shutdown];
    AH --> AI[End];

Class Structure

classDiagram
    class CameraNode {
        -Node node
        -camera_publisher
        -camera_info_publisher
        -camera_device
        -frame_id
        -camera_info
        +__init__()
        +start_capture()
        +stop_capture()
        -capture_loop()
        -process_frame()
    }

    class CameraDevice {
        +open()
        +close()
        +read_frame()
        +set_parameters()
        +get_parameters()
    }

    class ImageProcessor {
        +process_frame()
        +convert_format()
        +apply_calibration()
        +detect_features()
    }

    CameraNode --> CameraDevice : controls
    CameraNode --> ImageProcessor : uses

Component Interaction

flowchart TD
    subgraph CameraNode
        CD[Camera Driver]
        IP[Image Processor]
        CP[Camera Publisher]
        CI[Camera Info]
    end

    subgraph Hardware
        CAM[Camera Device]
        PARAMS[Parameters]
    end

    subgraph Topics
        IMG[Image Topic]
        INFO[Camera Info]
        FEAT[Features]
    end

    CAM -->|raw frames| CD
    CD -->|frames| IP
    PARAMS -->|config| CD
    IP -->|processed| CP
    CI -->|info| INFO
    CP -->|publish| IMG & FEAT

    style CameraNode fill:#bbf,stroke:#333,stroke-width:2px
    style Hardware fill:#fbf,stroke:#333,stroke-width:2px
    style Topics fill:#fbb,stroke:#333,stroke-width:2px

Data Flow

sequenceDiagram
    participant Camera
    participant Driver
    participant Processor
    participant Publisher
    participant Subscribers

    loop Capture Loop
        Camera->>Driver: Capture Frame
        Driver->>Processor: Raw Frame
        Processor->>Processor: Apply Calibration
        Processor->>Processor: Convert Format
        Processor->>Publisher: Processed Frame
        Publisher->>Subscribers: Image Message
    end

Image Processing Pipeline

graph TD
    A[Raw Frame] --> B[Format Conversion]
    B --> C[Distortion Correction]
    C --> D[Color Balance]
    D --> E[Feature Detection]
    E --> F[Compression]
    F --> G[ROS Message]

    subgraph Parameters
        P1[Camera Matrix]
        P2[Distortion Coeffs]
        P3[Color Params]
    end

    P1 & P2 --> C
    P3 --> D

State Machine

stateDiagram-v2
    [*] --> Initializing
    Initializing --> DeviceOpen: Init Success
    DeviceOpen --> Streaming: Start Capture
    Streaming --> DeviceOpen: Stop Capture
    DeviceOpen --> Error: Device Error
    Error --> DeviceOpen: Recovery
    DeviceOpen --> [*]: Shutdown

    state Streaming {
        [*] --> Capturing
        Capturing --> Processing
        Processing --> Publishing
        Publishing --> Capturing
    }

Error Handling

flowchart TD
    A[Error Detected] --> B{Error Type}
    B -->|Device Error| C[Reset Device]
    B -->|Frame Error| D[Skip Frame]
    B -->|Processing Error| E[Use Raw Frame]
    B -->|Publishing Error| F[Retry Publish]
    C --> G[Reinitialize]
    D & E & F --> H[Continue]
    G -->|Success| H
    G -->|Failure| I[Error State]

Configuration

Camera Parameters

graph LR
    subgraph Image
        I1[Resolution]
        I2[Framerate]
        I3[Format]
    end

    subgraph Calibration
        C1[Camera Matrix]
        C2[Distortion]
        C3[Rectification]
    end

    subgraph Processing
        P1[Color Balance]
        P2[Exposure]
        P3[Gain]
    end

    style Image fill:#bbf
    style Calibration fill:#fbf
    style Processing fill:#fbb

Topics

Published Topics

• /camera/image_raw (sensor_msgs/Image)
  • Raw camera frames
• /camera/camera_info (sensor_msgs/CameraInfo)
  • Camera calibration and parameters
• /camera/features (vision_msgs/Detection2DArray)
  • Detected features (optional)

Parameters

• camera_name (string)
• device_id (int)
• frame_id (string)
• image_width (int)
• image_height (int)
• framerate (int)
• calibration_file (string)

Dependencies

• ROS2 Packages:
  • rclpy
  • sensor_msgs
  • vision_msgs
  • cv_bridge
  • image_transport
• Python Libraries:
  • opencv-python
  • numpy
  • yaml

Notes

• Supports multiple camera types
• Real-time image processing
• Camera calibration handling
• Feature detection capabilities
• Dynamic parameter reconfiguration
• Error recovery mechanisms
• Performance optimization
• Timestamp synchronization
• Multiple output formats
• Resource management