head_node.md - dingdongdengdong/astra_ws GitHub Wiki

graph TD
    A[Start] --> B{Initialize ROS 2};
    B --> C[Create head_node];
    C --> D[Get Logger];
    D --> E[Declare Parameter device];
    E --> F[Get Parameter Value];
    F --> G[Define Joint Names];
    G --> H[Instantiate HeadController];
    H --> I[Create JointState Publisher];
    I --> J[Assign State Callback to HeadController];
    J --> K[Create Joint Command Subscriber];
    K --> L[Create Torque Enable Subscriber];
    L --> M[rclpy.spin node];

    M --> N[State Callback Triggered by HeadController];
    N --> O[Construct JointState Msg];
    O --> P[Publish JointState Msg];
    P --> M;

    M --> Q[Joint Command Callback Triggered by Subscription];
    Q --> R{Msg Joint Names and Length Valid?};
    R -- Yes --> S[Extract Position Command];
    S --> T[Call head_controller.set_pos];
    T --> M;
    R -- No --> M;

    M --> U[Torque Enable Callback Triggered by Subscription];
    U --> V[Log Torque State];
    V --> W[Call head_controller.set_torque];
    W --> M;

    M --> X[Keyboard Interrupt];
    X --> Y[Call head_controller.stop];
    Y --> Z[Raise KeyboardInterrupt];

    M --> AA[Node Shutdown];
    AA --> AB[ROS 2 Shutdown];
    AB --> AC[End];