code detail

leader_arm_controller

teleop_leader_arm

Description

---

The src/astra_controller/astra_controller/experiments # directory contains experimental ROS 2 nodes and related code for teleoperating the Astra robot. These nodes are not part of the core controller functionality but provide ways to test and explore different teleoperation methods. The directory includes implementations for controlling the robot using a SpaceMouse and a leader arm.

content

---

teleop_spacemouse_node.py: A ROS 2 node that allows teleoperation of the robot using a SpaceMouse. It reads SpaceMouse input and publishes goal poses and gripper commands for the robot arms.

spacemouse_agent.py: A class that acts as an agent to interface with the SpaceMouse expert.

spacemouse_expert.py: A class that provides an interface to the SpaceMouse device, reading its state and providing actions and button states.

teleop_leader_arm_node.py: A ROS 2 node that enables teleoperation of the robot by mimicking the movements of a leader arm. It reads the leader arm's joint states and gripper state, then publishes corresponding goal poses and gripper commands for the robot.

leader_arm_controller.py: A class to interface with the leader arm device.

System Overview

---

The experimental setup consists of several key components that work together to enable teleoperation of a robotic arm using a SpaceMouse input device.

graph TB
    SM[SpaceMouse Hardware] --> SME[SpaceMouseExpert]
    SME --> SMA[SpaceMouseAgent]
    SMA --> TN[Teleop Node]
    TN --> AC[ArmController]
    AC --> RA[Robot Arm Hardware]
    
    subgraph Hardware Layer
        SM
        RA
    end
    
    subgraph Interface Layer
        SME
        AC
    end
    
    subgraph Control Layer
        SMA
        TN
    end

Component Details

1. ArmController (leader_arm_controller.py)

The ArmController is responsible for low-level communication with the robot arm hardware.

classDiagram
    class ArmController {
        +COMM_LEN: int
        +COMM_HEAD: int
        +COMM_TYPE_*: int
        +GRIPPER_GEAR_R: float
        -ser: Serial
        -lock: Lock
        -last_position: array
        -last_velocity: array
        -last_effort: array
        -last_time: float
        +__init__(name: str)
        +get_pos()
        +set_pos(pos: array)
        +set_torque(torque: int)
        +stop()
    }

Key features:

• Serial communication with the arm at 921600 baud
• Position, velocity, and effort tracking
• Thread-safe operations
• Built-in safety limits for joint positions

2. SpaceMouse Interface (spacemouse_expert.py & spacemouse_agent.py)

classDiagram
    class SpaceMouseExpert {
        -state_lock: Lock
        -latest_data: dict
        -thread: Thread
        +__init__()
        -_read_spacemouse()
        +get_action()
    }
    
    class SpaceMouseAgent {
        -mouse: SpaceMouseExpert
        +__init__()
        +act(observation)
    }
    
    SpaceMouseAgent --> SpaceMouseExpert

The SpaceMouse interface consists of:

• SpaceMouseExpert: Low-level interface to the SpaceMouse hardware
• SpaceMouseAgent: High-level wrapper providing a clean action interface

3. Teleoperation Node (teleop_leader_arm_node.py)

graph TB
    subgraph ROS2 Node Structure
        TF[TF Listener] --> Node
        Node --> Publishers
        Publishers --> GP[Goal Pose]
        Publishers --> GPI[Goal Pose Inactive]
        Publishers --> CP[Camera Pose]
        Publishers --> GJ[Gripper Joint Command]
        Publishers --> AJ[Arm Joint Command]
        Publishers --> LJ[Lift Joint Command]
    end

Key features:

• ROS2 node implementation
• Transform management
• Multiple publisher types for different control aspects
• URDF-based kinematics

Data Flow

sequenceDiagram
    participant SM as SpaceMouse
    participant Agent as SpaceMouseAgent
    participant Node as Teleop Node
    participant Arm as ArmController
    participant ROS as ROS2 Topics
    
    loop Control Loop
        SM->>Agent: Raw Input
        Agent->>Node: Processed Action
        Node->>Arm: Joint Commands
        Arm->>Node: State Feedback
        Node->>ROS: Publish Status
    end

Communication Protocol

The ArmController uses a custom protocol for communication with the robot arm

graph LR
    subgraph Protocol Structure
        H[Header 0x5A] --> T[Type]
        T --> D[Data 14 bytes]
    end

Message Types:

• PING (0x00)
• PONG (0x01)
• CTRL (0x02)
• FEEDBACK (0x03)
• TORQUE (0x04)

Safety Features

The system implements several safety measures

1. Joint Position Limits

JOINT_MIN = np.array([-math.pi/2, -math.pi/2, -math.pi/2, -math.pi*0.75, -math.pi/2, -math.pi*0.75, -math.pi/2])
JOINT_MAX = np.array([math.pi/2, math.pi/2, math.pi/2, math.pi*0.75, math.pi/2, math.pi*0.75, math.pi/2])

2. Thread-safe operations with mutex locks
3. Automatic torque disable on shutdown
4. Gripper position calibration and limits

Usage

To use the teleoperation system

1. Ensure the SpaceMouse is connected
2. Launch the ROS2 node:

ros2 run astra_controller teleop_leader_arm_node

3. The system will wait for initial gripper calibration
4. Control the robot using the SpaceMouse input device