graph TD
%% External ROS topics
A1[Topic: arm/joint_command]
A2[Topic: arm/gripper_joint_command]
A3[Topic: lift/joint_command]
E[Topic: joint_states]
%% Main node initialization
P[Start: dry_run_node] -->|Initialize ROS 2| Q[Load Parameters]
Q -->|actively_send_joint_state, joint_names| R[Create Publisher]
R -->|JointState| E
Q -->|Create Subscribers| S[Setup Subscriptions]
S -->|JointCommand| A1
S -->|JointCommand| A2
S -->|JointCommand| A3
%% Subgraph for command processing
subgraph Command Processing
A1 -->|Arm Command| B[Process Arm Command]
A2 -->|Gripper Command| C[Process Gripper Command]
A3 -->|Lift Command| D[Process Lift Command]
B -->|Joint Names, Position| G[Extract Arm Data]
C -->|Joint Names, Position| H[Extract Gripper Data]
H -->|Map 1 Pos to 2| H1[Gripper Joint Positions]
D -->|Joint Names, Position| I[Extract Lift Data]
G -->|Combine Data| J[Assemble JointState]
H1 -->|Combine Data| J
I -->|Combine Data| J
J -->|JointState| K[Populated JointState]
K -->|Publish| E
end
%% Subgraph for active sending (conditional)
subgraph Active Sending
Q -->|If actively_send_joint_state| L[Initialize Joint States Dict]
K -->|If actively_send_joint_state| M[Update Joint States Dict]
M -->|Store| N[Internal: joint_states]
N -->|Periodic Read| O[Prepare JointState from Dict]
O -->|JointState| E
end
%% Styling for clarity
classDef node fill:#f0f9ff,stroke:#333,stroke-width:2px
classDef topic fill:#ccffcc,stroke:#333,stroke-width:1px
classDef highlight fill:#fff3cd,stroke:#d4a017,stroke-width:2px
class P,Q,R,S,B,C,D,G,H,H1,I,J,K,L,M,N,O node
class A1,A2,A3,E topic
class J,K highlight
linkStyle 9,10,11,12 stroke:#d4a017,stroke-width stroke:#d4a017,stroke-width:2px
linkStyle 14 stroke:#d4a017,stroke-width:2px,stroke-dasharray:5,5
%% Comments for clarity
%% dry_run_node: Simulates joint states from arm, gripper, lift commands.
%% Command Processing: Converts JointCommand to JointState for publishing.
%% Active Sending: Optionally maintains and publishes internal joint states.
graph TD
A[Start] --> B{Initialize ROS 2};
B --> C[Create dry_run_node];
C --> D[Create joint_states Publisher];
D --> E[Declare Parameters];
E --> F[Get Parameters];
F --> G{Assert 8 Joint Names};
G --> H{actively_send_joint_state is True?};
H -- Yes --> I[Initialize joint_states Dict];
I --> J[Create Publish Thread];
J --> K[Start Publish Thread];
K --> L[Publish JointState Msg Periodically];
L --> M[Sleep 0.1s];
M --> L;
H -- No --> N[Skip Publish Thread];
N --> O[Create Arm Command Subscriber];
K --> O;
O --> P[Arm Command Callback];
P --> Q{Assert Arm Joint Names};
Q --> R[Create JointState Msg for Arm];
R --> S[Set Arm Joint Positions];
S --> T[Publish Arm JointState];
T --> U{actively_send_joint_state is True?};
U -- Yes --> V[Update joint_states with Arm Positions];
V --> W[Create Gripper Command Subscriber];
U -- No --> W;
W --> X[Gripper Command Callback];
X --> Y{Assert Gripper Joint Name};
Y --> Z[Create JointState Msg for Gripper];
Z --> AA[Set Gripper Joint Positions Mirrored];
AA --> AB[Publish Gripper JointState];
AB --> AC{actively_send_joint_state is True?};
AC -- Yes --> AD[Update joint_states with Gripper Positions];
AD --> AE[Create Lift Command Subscriber];
AC -- No --> AE;
AE --> AF[Lift Command Callback];
AF --> AG{Assert Lift Joint Name};
AG --> AH[Create JointState Msg for Lift];
AH --> AI[Set Lift Joint Positions];
AI --> AJ[Publish Lift JointState];
AJ --> AK{actively_send_joint_state is True?};
AK -- Yes --> AL[Update joint_states with Lift Position];
AL --> AM[rclpy.spin node];
AK -- No --> AM;
AM --> AN[Node Shutdown];
AN --> AO[ROS 2 Shutdown];
AO --> AP[End];