ROS 로봇 팔 전체 제어 프로세스 — 비동기 제어 루프의 이해

flowchart TD
    A["① /joint_states 구독<br/>(콜백으로 최신 관절값 저장)"] --> B["② 목표 Pose 계산"]
    B --> C["③ /compute_ik 호출<br/>(시드 = 최신 관절각)"]
    C --> D["④ 결과 관절각 발행"]
    D --> E["⑤ 컨트롤러가 모터 구동"]
    E --> F["⑥ 컨트롤러가 /joint_states 갱신"]
    F --> A
 
    style A fill:#4CAF50,color:#fff
    style D fill:#2196F3,color:#fff
    style E fill:#FF9800,color:#fff

sequenceDiagram
    participant CB as 콜백 흐름
    participant CL as 제어 루프
    participant CT as 컨트롤러
 
    loop 항상 반복
        CT->>CB: /joint_states 발행
        CB->>CB: self._current_joints 갱신
    end
 
    loop 30Hz 반복
        CL->>CL: 목표 Pose 계산
        CL->>CL: IK 호출 (시드 = 최신 관절값)
        CL->>CT: 관절각 명령 발행
        CT->>CT: 모터 구동
    end

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
 
class ArmController(Node):
    def __init__(self):
        super().__init__('arm_controller')
        self._current_joints = {}
 
        # ① 콜백 등록: /joint_states가 올 때마다 자동 실행
        self.create_subscription(
            JointState, '/joint_states', self._joint_state_cb, 10
        )
 
        # 30Hz 타이머로 제어 루프 실행
        self.create_timer(1.0 / 30, self._control_loop)
 
    def _joint_state_cb(self, msg):
        """콜백: /joint_states가 올 때마다 자동 실행 (별도 흐름)"""
        for name, pos in zip(msg.name, msg.position):
            self._current_joints[name] = pos  # 최신 값 저장
 
    def _control_loop(self):
        """제어 루프: 30Hz로 반복"""
        target = self._next_target_pose()                    # ② 목표 계산
        ik_result = self._compute_ik(target, self._current_joints)  # ③ IK (최신 시드 사용)
        self._publish_command(ik_result)                     # ④ 발행
        # ⑤⑥ 컨트롤러가 모터 구동 + joint_states 갱신 → 콜백이 알아서 받음

flowchart LR
    subgraph 내노드["내 노드 (Planner)"]
        A[목표 Pose 계산] --> B[/compute_ik 호출/]
        B --> C[관절각 발행]
    end
 
    subgraph 컨트롤러["컨트롤러 (Hardware Interface)"]
        D[모터 구동] --> E[/joint_states 발행/]
        E --> F[TF 갱신]
    end
 
    C -->|"명령 토픽"| D
    E -->|"상태 토픽"| A
 
    style 내노드 fill:#E3F2FD,stroke:#1565C0
    style 컨트롤러 fill:#FFF3E0,stroke:#E65100

def _control_loop(self):
    # 초기값 체크: joint_states가 아직 안 왔으면 스킵
    if not self._current_joints:
        self.get_logger().warn('Waiting for /joint_states...')
        return
 
    target = self._next_target_pose()
    ik_result = self._compute_ik(target, self._current_joints)
    self._publish_command(ik_result)