arm_node.md - dingdongdengdong/astra_ws GitHub Wiki

graph TD
    A[Start] --> B{Initialize ROS 2};
    B --> C[Create arm_node];
    C --> D[Get Logger];
    D --> E[Declare Parameters];
    E --> F[Get Parameter Values];
    F --> G{Assert Joint Names Lengths};
    G --> H[Instantiate ArmController];
    H --> I[Create JointState Publisher Arm];
    H --> J[Create JointState Publisher Gripper];
    H --> K[Create Debug Publisher];
    H --> L[Create Pong Publisher];
    H --> M[Create Error Publisher];
    M --> N[Assign State Callback to ArmController];
    N --> O[Assign Debug Callback to ArmController];
    O --> P[Assign Pong Callback to ArmController];
    P --> Q[Assign Error Callback to ArmController];
    Q --> R[Get Initial Position from ArmController];
    R --> S[Create Arm Joint Command Subscriber];
    S --> T[Create Gripper Command Subscriber];
    T --> U[Create Torque Enable Subscriber];
    U --> V[Create Ping Subscriber];
    V --> W[Create Set PID Subscriber];
    W --> X[rclpy.spin node];
    X --> Y[Arm State Callback Triggered];
    Y --> Z[Construct Arm JointState Msg];
    Z --> AA[Publish Arm JointState];
    AA --> AB[Construct Gripper JointState Msg];
    AB --> AC[Publish Gripper JointState];
    AC --> X;

    X --> AD[Arm Joint Command Callback Triggered];
    AD --> AE{Verify Arm Joint Names};
    AE --> AF[Update last_position_cmd Arm];
    AF --> AG[Call arm_controller.set_pos];
    AG --> AH[Update last_position_cmd];
    AH --> X;

    X --> AI[Gripper Command Callback Triggered];
    AI --> AJ{Verify Gripper Joint Name};
    AJ --> AK[Update last_position_cmd Gripper];
    AK --> AL[Call arm_controller.set_pos];
    AL --> AM[Update last_position_cmd];
    AM --> X;

    X --> AN[Torque Enable Callback Triggered];
    AN --> AO[Log Torque State];
    AO --> AP[Call arm_controller.set_torque];
    AP --> X;

    X --> AQ[Ping Callback Triggered];
    AQ --> AR[Log Ping Data];
    AR --> AS[Construct Ping Packet];
    AS --> AT[Call arm_controller.write];
    AT --> X;

    X --> AU[Set PID Callback Triggered];
    AU --> AV[Log PID Data];
    AV --> AW[Call arm_controller.set_pid];
    AW --> X;

    X --> AX[Debug Callback Triggered];
    AX --> AY[Construct Float32MultiArray Msg];
    AY --> AZ[Publish Debug Msg];
    AZ --> X;

    X --> BA[Pong Callback Triggered];
    BA --> BB[Log Pong Data];
    BB --> BC[Construct UInt16MultiArray Msg];
    BC --> BD[Publish Pong Msg];
    BD --> X;

    X --> BE[Keyboard Interrupt];
    BE --> BF[Call arm_controller.stop];
    BF --> BG[Raise KeyboardInterrupt];

    X --> BH[Node Shutdown];
    BH --> BI[ROS 2 Shutdown];
    BI --> BJ[End];