Code Flow Analysis: leader_arm_controller.py

Code Flow Diagram

flowchart TD
    A[Start: Import Modules] --> B[Class ArmController Definition]
    B --> C[__init__ Method: Serial Setup, Thread Start]
    C --> D[recv_thread: Serial Data Receive Loop]
    D -->|PONG| E[pong_cb Callback]
    D -->|FEEDBACK| F[State Update: Position, Velocity, Effort]
    F --> G[state_cb Callback]
    D --> H[Debug Data Handling]
    D --> I[Other Data: Ignored]
    B --> J[get_pos: Return Last State]
    B --> K[write: Serial Write]
    B --> L[set_pos: Position Command]
    B --> M[set_torque: Torque Command]
    B --> N[stop: Cleanup and Serial Close]
    B --> O[__del__: Destructor Calls stop]
    B --> P[to_si_unit / to_raw_unit: Data Conversion]

Detailed Code Flow Explanation

1. Module Imports and Logging

• Imports standard libraries (serial, threading, numpy, struct, math, time, sys, logging).
• Configures logging for the module.

2. ArmController Class Definition

• Constants: Defines communication protocol constants and gripper gear ratio.
• Static Methods:
  • to_si_unit(arr): Converts raw joint data to SI units, with special handling for the gripper.
  • to_raw_unit(arr): Converts SI units back to raw protocol format.

3. Initialization (__init__)

• Opens a serial connection to the specified device.
• Initializes state variables and threading locks.
• Starts a background thread (recv_thread) for receiving data.
• Waits for initial position data before proceeding.

4. Data Reception Thread (recv_thread)

• Continuously reads from the serial port unless signaled to quit.
• Handles three main data types:
  • PONG: Calls pong_cb callback with unpacked data.
  • FEEDBACK:
    • Converts raw data to SI units.
    • Computes velocity and effort.
    • Updates last known state.
    • Calls state_cb callback if set.
  • Debug Data: Handles ASCII debug output, calls debug_cb if set.
  • Other Data: Ignored or printed for debugging.

5. State Access and Command Methods

• get_pos(): Returns the latest position, velocity, effort, and timestamp (thread-safe).
• write(encoded_data): Sends encoded data to the serial port (thread-safe).
• set_pos(pos): Clips position to joint limits, encodes, and sends position command.
• set_torque(torque): Sends a torque command.
• stop(): Signals thread to quit, sets torque to zero, flushes and closes serial port.
• __del__(): Ensures cleanup by calling stop() on object destruction.

6. Data Conversion

• to_si_unit and to_raw_unit handle conversion between protocol and SI units, including gripper calibration.

7. Thread Safety

• Uses locks to ensure thread-safe access to shared state and serial writes.

8. Callbacks

• Supports user-defined callbacks for state updates, pong responses, configuration, and debug data.

9. Control Flow

• The main control flow is event-driven, based on serial data reception and user commands via method calls.

---