ik_node.md - dingdongdengdong/astra_ws GitHub Wiki

focused on goal_pose

graph TD
    %% Start with goal_pose topic
    A[Topic: goal_pose] -->|PoseStamped| B[Callback: cb]

    %% Subgraph for goal pose processing
    subgraph Goal Pose Processing
        B -->|Extract pose| C[Convert to pq Array]
        C -->|pq Array| D[Convert to Transform Matrix]
        D -->|Matrix| E[Set EE Pose Matrix]
        E -->|Store| F[Target: ee_pose_matrix]
        E -->|Enable| G[Set target_set: True]
        G --> H[Activate IK Timer]
    end

    %% Subgraph for IK solving
    subgraph IK Solving
        H -->|Timer Fires| I[Callback: solve_ik]
        I --> J{Target Set?}
        J -->|No| K[Log: No Target Pose]
        J -->|Yes| L[Retrieve Target Matrix]
        L --> M[Compute IK: modern_robotics]
        M -->|theta_list, success| N[Check Joint Limits]
        N --> O{Limits OK and IK Success?}
        O -->|Yes| P[Normalize Joint Angles]
        P --> Q[Update last_theta_list]
        Q --> R[Publish: AstraJointState]
        O -->|No: Limits or IK Failed| S[Log: Error or Warning]
        S --> T[Publish: Error]
    end

    %% Output topics
    R -->|joint_states| U[Topic: joint_states]
    T -->|error| V[Topic: error]

    %% Styling for clarity
    classDef node fill:#f0f9ff,stroke:#333,stroke-width:2px
    classDef decision fill:#e6f3ff,stroke:#005,stroke-width:2px
    classDef topic fill:#ccffcc,stroke:#333,stroke-width:1px
    classDef highlight fill:#fff3cd,stroke:#d4a017,stroke-width:2px
    class A,B,C,D,E,F,G,H,I,J,K,L,M,N,O,P,Q,R,S,T,U,V node
    class J,O decision
    class A,U,V topic
    class O,P,Q,R highlight
    linkStyle 10,11,12 stroke:#d4a017,stroke-width:2px

    %% Comments for clarity
    %% Goal Pose Processing: Convert incoming PoseStamped to a target matrix.
    %% IK Solving: Periodically compute IK, publish joint states or errors.