moveit_relay_node.md - dingdongdengdong/astra_ws GitHub Wiki

graph TD
    A[Start] --> B{Initialize ROS 2};
    B --> C[Create moveit_relay_node];
    C --> D[Create Arm JointCommand Publisher];
    C --> E[Create Gripper JointCommand Publisher];
    C --> F[Create Lift JointCommand Publisher];
    D --> G[Initialize joint_pos Dictionary];
    E --> G;
    F --> G;
    G --> H[Create Arm FollowJointTrajectory Action Server];
    H --> I[Arm Action execute_callback triggered];
    I --> J[Log Executing Goal];
    J --> K[Update joint_pos with Arm and Lift Target Positions];
    K --> L[Create Arm JointCommand Msg];
    L --> M[Publish Arm JointCommand];
    M --> N[Create Lift JointCommand Msg];
    N --> O[Publish Lift JointCommand];
    O --> P[Succeed Arm Goal];
    P --> Q[Return Success Result];
    Q --> R[Create Gripper FollowJointTrajectory Action Server];
    R --> S[Gripper Action execute_callback triggered];
    S --> T[Log Executing Goal];
    T --> U[Update joint_pos with Gripper Target Position];
    U --> V[Create Gripper JointCommand Msg];
    V --> W[Publish Gripper JointCommand];
    W --> X[Succeed Gripper Goal];
    X --> Y[Return Success Result];
    Y --> Z[rclpy.spin node];
    Z --> AA[Node Shutdown];
    AA --> AB[ROS 2 Shutdown];
    AB --> AC[End];

    I --> Z;
    S --> Z;

dataflow

graph TD
    %% Main node initialization
    A[Start: moveit_relay_node] -->|Initialize ROS 2| B[Create Node: moveit_relay_node]

    %% Subgraph for communication interfaces
    subgraph Communication Interfaces
        B -->|Create Publishers| C[Setup Topics]
        C -->|AstraJointState| C1[Topic: arm/joint_command]
        C -->|AstraJointState| C2[Topic: arm/gripper_joint_command]
        C -->|AstraJointState| C3[Topic: lift/joint_command]

        B -->|Create Action Server| D[Setup Action]
        D -->|FollowJointTrajectory| D1[Action: astra_right_hand_controller]
    end

    %% Subgraph for action goal processing
    subgraph Action Goal Processing
        D1 -->|Goal: FollowJointTrajectory| E[Callback: execute_callback]
        E -->|Extract Data| F[Get Final Joint Positions]
        F -->|Joint Positions| G[Map to Controllers]
        G -->|Gripper Position| H[Prepare Gripper JointState]
        H -->|Publish| C2
        G -->|Lift Position| I[Prepare Lift JointState]
        I -->|Publish| C3
        E -->|Set Result| J[Mark Goal Successful]
        J -->|Succeed| D1
    end

    %% Styling for clarity
    classDef node fill:#f0f9ff,stroke:#333,stroke-width:2px
    classDef topic fill:#ccffcc,stroke:#333,stroke-width:1px
    classDef action fill:#ffccff,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 node
    class C1,C2,C3 topic
    class D1 action
    class H,I highlight
    linkStyle 6,7 stroke:#d4a017,stroke-width:2px

    %% Comments for clarity
    %% Note: arm/joint_command is created but unused in the provided snippet.
    %% Node initializes publishers for arm, gripper, lift, and an action server.
    %% Action goals are processed to publish gripper and lift commands.