Code Flow Analysis: teleop_leader_arm_node.py

Code Flow Diagram

flowchart TD
    A[Start: Import Modules] --> B[Define QoS Profiles, Utility Functions]
    B --> C[main Function]
    C --> D[rclpy.init]
    D --> E[Create ROS2 Node]
    E --> F[Setup Publishers for Each Arm Side]
    F --> G[Define pub_goal_cb, pub_gripper_cb]
    G --> H[Setup TF2 Buffer and Listener]
    H --> I[Wait for Initial Transform]
    I --> J[Load URDF, Kinematics]
    J --> K[Create ArmController Instance]
    K --> L[get_leader_arm_state Function]
    L --> M[Gripper Open Wait Loop]
    M --> N[Gripper Close Wait Loop]
    N --> O[Main Control Loop:]
    O --> P[Read Arm State]
    P --> Q[Forward Kinematics]
    Q --> R[Transform Calculations]
    R --> S[Publish Goal Pose and Gripper Command]
    S --> O
    O --> T[rclpy.spin]
    T --> U[Node Cleanup]

Detailed Code Flow Explanation

1. Module Imports and QoS Profile Setup

• Imports ROS2, geometry, and custom message types, kinematics libraries, and utility modules.
• Defines a reliable QoS profile for sensor data.

2. Utility Functions

• clip(x, mn, mx): Clamps a value between minimum and maximum.

3. main() Function

• ROS2 Initialization:

  • Initializes ROS2 and creates a node.
  • Sets up logging.

• Publisher Setup:

  • Creates publishers for goal poses, camera poses, and joint commands for both left and right arms.

• Callback Definitions:

  • pub_goal_cb: Publishes pose messages for goals and camera.
  • pub_gripper_cb: Publishes gripper joint commands.

• TF2 Listener:

  • Sets up a TF2 buffer and listener to obtain transforms.
  • Waits for the transform from base_link to link_ree_teleop to become available.

• URDF and Kinematics:

  • Loads robot URDF and extracts kinematic parameters for forward kinematics.

• Arm Controller:

  • Instantiates ArmController for serial communication with the physical arm.
  • Defines get_leader_arm_state() to read joint and gripper state, with gripper normalization.

• Gripper State Synchronization:

  • Waits for the gripper to be opened and then closed, using polling loops.

• Initial Transform Calculation:

  • Computes the initial end-effector pose and its inverse for later use.

• Main Control Loop:

  • Continuously reads the leader arm state.
  • Computes the current end-effector pose using forward kinematics.
  • Calculates the transformation from the initial pose.
  • Scales and applies the transformation to the goal pose.
  • Publishes the goal pose and gripper command to the right arm.
  • Sleeps for the control period and repeats.

• ROS2 Spin and Cleanup:

  • Calls rclpy.spin(node) to process callbacks (though the main loop is blocking).
  • Cleans up the node and shuts down ROS2 on exit.

4. Data Flow and Control

• The code is structured around ROS2 node lifecycle and real-time polling of the leader arm state.
• Publishes pose and joint commands based on live kinematic calculations.
• Uses serial communication for hardware feedback and ROS2 for distributed control.

---