Web Teleoperation Node (teleop_web_node.py)

flow chart

graph TD
    A[Start] --> B{Initialize ROS 2};
    B --> C[Create teleop_web_node];
    C --> D[Get Logger];
    D --> E[Create Teleopoperator Instance];
    E --> F[Create TF Buffer and Listener];
    F --> G[Assign get_current_eef_pose_cb to Teleopoperator];
    G --> H[Load URDF and Setup Kinematics M and Slist];
    H --> I[Assign get_initial_eef_pose_cb to Teleopoperator];
    I --> J[Define pub_T Helper Function];
    J --> K[Create Publisher Dictionaries];
    K --> L[Create Publishers for Each Side];
    L --> M[Assign pub_goal_cb to Teleopoperator];
    M --> N[Assign pub_gripper_cb to Teleopoperator];
    N --> O[Create Head Joint Command Publisher];
    O --> P[Assign pub_head_cb to Teleopoperator];
    P --> Q[Create cmd_vel Publisher];
    Q --> R[Assign cmd_vel_cb to Teleopoperator];
    R --> S[Create Reset Publisher];
    S --> T[Create Done Publisher];
    T --> U[Assign reset_cb to Teleopoperator];
    U --> V[Assign done_cb to Teleopoperator];
    V --> W[Define get_cb Factory Function];
    W --> X[Create Camera Image Subscribers];
    X --> Y[Create Error Subscribers];
    Y --> Z[rclpy.spin node];

    Z --> AA[get_current_eef_pose_cb Triggered by Teleopoperator];
    AA --> AB[Lookup TF Transform base_link to EEF];
    AB --> AC[Convert ROS Transform to 4x4 Matrix];
    AC --> AD[Return EEF Pose];
    AD --> Z;

    Z --> AE[get_initial_eef_pose_cb Triggered by Teleopoperator];
    AE --> AF[Calculate FK using Modern Robotics];
    AF --> AG[Return Initial EEF Pose];
    AG --> Z;

    Z --> AH[pub_goal_cb Triggered by Teleopoperator];
    AH --> AI{Tsgoal is not None?};
    AI -- Yes --> AJ[Call pub_T with goal_pose_publisher];
    AI -- No --> AK{Tsgoal_inactive is not None?};
    AJ --> AK;
    AK -- Yes --> AL[Call pub_T with goal_pose_inactive_publisher];
    AK -- No --> AM{Tscam is not None?};
    AL --> AM;
    AM -- Yes --> AN[Call pub_T with cam_pose_publisher];
    AN --> Z;
    AM -- No --> Z;

    Z --> AO[pub_gripper_cb Triggered by Teleopoperator];
    AO --> AP[Construct Gripper JointCommand Msg];
    AP --> AQ[Publish Gripper JointCommand];
    AQ --> Z;

    Z --> AR[pub_head_cb Triggered by Teleopoperator];
    AR --> AS[Construct Head JointCommand Msg];
    AS --> AT[Publish Head JointCommand];
    AT --> Z;

    Z --> AU[cmd_vel_cb Triggered by Teleopoperator];
    AU --> AV[Construct Twist Msg];
    AV --> AW[Publish Twist Msg];
    AW --> Z;

    Z --> AX[reset_cb Triggered by Teleopoperator];
    AX --> AY[Construct Bool Msg True];
    AY --> AZ[Publish Reset Msg];
    AZ --> Z;

    Z --> BA[done_cb Triggered by Teleopoperator];
    BA --> BB[Construct Bool Msg True];
    BB --> BC[Publish Done Msg];
    BC --> Z;

    Z --> BD[Camera Image Callback Triggered by Subscription];
    BD --> BE{Msg Encoding and Dimensions Valid?};
    BE -- Yes --> BF[Convert Image to NumPy Array];
    BF --> BG[Send Image Data to Teleopoperator Webserver];
    BG --> Z;
    BE -- No --> Z;

    Z --> BH[Error Callback Triggered by Subscription];
    BH --> BI[Forward Error to Teleopoperator error_cb];
    BI --> Z;

    Z --> BJ[Keyboard Interrupt];
    BJ --> BK[Stop Teleopoperator Webserver Loop];
    BK --> BL[Raise KeyboardInterrupt];

    Z --> BM[Node Shutdown];
    BM --> BN[ROS 2 Shutdown];
    BN --> BO[End];

Class Structure

classDiagram
    class WebTeleopNode {
        -Node node
        -web_server
        -camera_subscriber
        -joint_state_publisher
        -pose_publisher
        -websocket_connections
        +__init__()
        +start_server()
        +stop_server()
        -handle_websocket()
        -process_command()
        -publish_feedback()
    }

    class WebServer {
        +start()
        +stop()
        +handle_connection()
        +broadcast_state()
    }

    class CommandProcessor {
        +process_joint_command()
        +process_cartesian_command()
        +validate_command()
    }

    class FeedbackHandler {
        +process_camera()
        +process_joint_state()
        +compress_image()
    }

    WebTeleopNode --> WebServer : manages
    WebTeleopNode --> CommandProcessor : uses
    WebTeleopNode --> FeedbackHandler : uses

Component Interaction

flowchart TD
    subgraph WebTeleopNode
        WS[Web Server]
        CP[Command Processor]
        FB[Feedback Handler]
        VP[Video Processor]
    end

    subgraph External
        WC[Web Clients]
        RC[Robot Control]
        CAM[Camera Feed]
    end

    subgraph Topics
        JP[Joint Publisher]
        PP[Pose Publisher]
        CS[Camera Stream]
    end

    WC -->|commands| WS
    WS -->|process| CP
    CP -->|validate| JP & PP
    JP & PP -->|control| RC
    CAM -->|stream| VP
    VP -->|compress| FB
    FB -->|feedback| WS
    WS -->|update| WC

    style WebTeleopNode fill:#bbf,stroke:#333,stroke-width:2px
    style External fill:#fbb,stroke:#333,stroke-width:2px
    style Topics fill:#fbf,stroke:#333,stroke-width:2px

Data Flow

sequenceDiagram
    participant Client
    participant WebSocket
    participant Processor
    participant ROS
    participant Camera

    Client->>WebSocket: Connect
    WebSocket->>Client: Connection Established
    loop Command Processing
        Client->>WebSocket: Send Command
        WebSocket->>Processor: Process Command
        Processor->>ROS: Publish Command
        Camera->>Processor: Stream Data
        Processor->>WebSocket: Compress & Send
        WebSocket->>Client: Update State
    end

Web Interface

graph TD
    subgraph Interface
        VC[Video Canvas]
        JC[Joint Control]
        CC[Cartesian Control]
        FB[Feedback Display]
    end

    subgraph Controls
        JS[Joint Sliders]
        KB[Keyboard Control]
        MS[Mouse Control]
        BT[Buttons]
    end

    subgraph Feedback
        VS[Video Stream]
        RS[Robot State]
        ST[Status]
    end

    JS & KB & MS -->|input| JC & CC
    JC & CC -->|command| BT
    VS -->|display| VC
    RS -->|update| FB
    ST -->|show| FB

    style Interface fill:#bbf
    style Controls fill:#fbf
    style Feedback fill:#fbb

State Machine

stateDiagram-v2
    [*] --> Starting
    Starting --> ServerReady: Server Started
    ServerReady --> Connected: Client Connected
    Connected --> Processing: Command Received
    Processing --> Connected: Command Complete
    Connected --> Disconnected: Client Lost
    Disconnected --> Connected: New Client
    Connected --> [*]: Shutdown

Error Handling

flowchart TD
    A[Error Detected] --> B{Error Type}
    B -->|Connection Lost| C[Cleanup Session]
    B -->|Invalid Command| D[Reject Command]
    B -->|Stream Error| E[Restart Stream]
    B -->|Server Error| F[Restart Server]
    C --> G[Wait Reconnect]
    D --> H[Notify Client]
    E --> I[Notify Client]
    F --> J[Emergency Stop]

Network Architecture

graph LR
    subgraph Server
        WS[WebSocket Server]
        RP[ROS Publisher]
        RS[ROS Subscriber]
    end

    subgraph Client
        WC[WebSocket Client]
        UI[User Interface]
        VD[Video Display]
    end

    subgraph Protocol
        CMD[Commands]
        ST[State]
        VID[Video]
    end

    WC -->|send| CMD
    CMD -->|process| WS
    WS -->|publish| RP
    RS -->|receive| ST
    ST -->|update| WC
    VID -->|stream| VD

    style Server fill:#bbf
    style Client fill:#fbf
    style Protocol fill:#fbb

Configuration

Server Parameters

graph LR
    subgraph Network
        P1[Port]
        P2[Host]
        P3[Max Clients]
    end

    subgraph Video
        V1[Resolution]
        V2[Framerate]
        V3[Quality]
    end

    subgraph Control
        C1[Rate Limits]
        C2[Timeouts]
        C3[Safety Checks]
    end

    style Network fill:#bbf
    style Video fill:#fbf
    style Control fill:#fbb

Dependencies

• ROS2 Packages:
  • rclpy
  • sensor_msgs
  • geometry_msgs
  • cv_bridge
• Python Libraries:
  • websockets
  • asyncio
  • opencv-python
  • numpy
  • json

Topics

Published Topics

• /cmd_joint (sensor_msgs/JointState)
• /cmd_pose (geometry_msgs/PoseStamped)

Subscribed Topics

• /camera/image_raw (sensor_msgs/Image)
• /joint_states (sensor_msgs/JointState)
• /robot_state (std_msgs/String)

Notes

• Implements WebSocket server for real-time control
• Supports multiple simultaneous clients
• Real-time video streaming with compression
• Joint and Cartesian space control
• Command validation and rate limiting
• Automatic reconnection handling
• Emergency stop functionality
• Cross-platform web interface
• Configurable video quality
• Secure WebSocket support