RF 조종 & ONNX 배포

학습된 ONNX 정책을 Rock 5C에 띄우고 ExpressLRS 조종기로 직접 걷게 만드는 end-to-end 흐름.

[mechadog RGB 카메라가 장애물을 YOLO로 인식하고 회피하는 일인칭 시점 렌더]
그림 6.1 — 카메라 장애물 인지 능동 회피 시연
[사람이 mechadog 옆구리를 밀어 자세가 흔들려도 다리로 자세를 보정하는 모습]
그림 6.2 — 외부 충격에 대한 자세 보정 시연(렌더링)

1. RF 조종 — 채널 매핑

Channel조종기 입력의미정책 출력
1 (X축 좌측 스틱)전 / 후vx 명령-1 ~ +1 m/s
2 (Y축 좌측 스틱)좌 / 우 스트레이프vy 명령-1 ~ +1 m/s
4 (X축 우측 스틱)회전yaw rate 명령-1 ~ +1 rad/s
5 (SA 3단 스위치)gait/mode0=앉기, 1=걷기, 2=뛰기gait_scale [0.0, 1.0, 1.7]
6 (SB 2단 스위치)E-stopOFF=비상정지, ON=활성force sit-down
7~16여유 (높이, 특수 동작)——
CRSF 채널 인덱스: 가장 왼쪽 AUX1 = 채널 5, 가장 왼쪽 AUX2 = 채널 6. 자세한 CRSF packet layout은 [tbs-crsf-spec](https://github.com/tbs-fpv/tbs-crsf-spec/blob/main/crsf.md) 참조.

2. CRSF 파서 (Python, Rock 5C)

# file: mechadog_control/crsf_bridge.py
import serial, struct, threading, time
from dataclasses import dataclass

CRSF_SYNC = 0xC8
RC_PAYLOAD_LEN = 22
RC_FRAME_TYPE  = 0x16

@dataclass
class RCState:
    vx: float = 0.0
    vy: float = 0.0
    w: float = 0.0
    gait: int = 1            # 0: sit, 1: walk, 2: run
    estop: bool = True       # True=armed
    fs_lost: bool = False
    last_link_ts: float = 0.0

def unpack_rc(payload22: bytes):
    """16 channels of 11-bit packed (big-endian)"""
    ch = []
    bits = int.from_bytes(payload22, "big")
    for i in range(16):
        ch.append((bits >> (11 * (15 - i))) & 0x7FF)
    return ch

def to_cmd(raw, lo=174, hi=992):
    return max(-1.0, min(1.0, (raw - lo) / (hi - lo) * 2.0 - 1.0))

class CRSFBridge:
    def __init__(self, port="/dev/ttyS4", baud=420000, fs_timeout=0.5):
        self.ser = serial.Serial(port, baud, timeout=0.01)
        self.buf = bytearray()
        self.state = RCState()
        self.fs_timeout = fs_timeout
        threading.Thread(target=self._run, daemon=True).start()

    def _run(self):
        while True:
            data = self.ser.read(64)
            if not data: continue
            self.buf.extend(data)
            self._parse_frames()

    def _parse_frames(self):
        while len(self.buf) >= 4:
            if self.buf[0] != CRSF_SYNC:
                self.buf.pop(0); continue
            length = self.buf[1]
            if length < 3 or len(self.buf) < length + 2:
                return
            ftype = self.buf[2]
            payload = self.buf[3:1+length]
            self.buf = self.buf[2+length:]
            if ftype == RC_FRAME_TYPE and len(payload) == RC_PAYLOAD_LEN:
                ch = unpack_rc(payload)
                self.state.last_link_ts = time.time()
                self.state.vx    = to_cmd(ch[0])
                self.state.vy    = to_cmd(ch[1])
                self.state.w     = to_cmd(ch[3])
                self.state.gait  = int(round((to_cmd(ch[4]) + 1) / 2 * 2))   # 0/1/2
                self.state.estop = to_cmd(ch[5]) > 0.05

    def safe_state(self):
        if time.time() - self.state.last_link_ts > self.fs_timeout:
            self.state.fs_lost = True
            return RCState(estop=False, fs_lost=True)  # 모든 명령 0 + 비상 정지
        return self.state

3. ONNX 추론 + 정책 (Rock 5C, ROS2 노드)

# file: mechadog_control/rl_policy_onnx_runner.py
import rclpy, onnxruntime as ort, numpy as np
from rclpy.node import Node
from sensor_msgs.msg import Imu, JointState
from geometry_msgs.msg import Twist
from std_msgs.msg import Float32MultiArray

class MechadogPolicyNode(Node):
    def __init__(self):
        super().__init__('mechadog_policy')
        # ONNX session 생성
        providers = ['CPUExecutionProvider']   # Rock 5C Lite NPU 사용 시 'RknpuExecutionProvider'
        self.sess = ort.InferenceSession('/opt/mechadog/policy.onnx', providers=providers)
        self.input_name = self.sess.get_inputs()[0].name
        self.output_name = self.sess.get_outputs()[0].name

        # 상태 버퍼
        self.base_lin_vel = np.zeros(3)
        self.base_ang_vel = np.zeros(3)
        self.proj_grav = np.array([0, 0, -1.0])
        self.joint_pos = np.zeros(12)
        self.joint_vel = np.zeros(12)
        self.last_action = np.zeros(12)

        # ROS2 sub/pub
        self.create_subscription(Imu, '/imu/data', self.imu_cb, 10)
        self.create_subscription(JointState, '/joint_states', self.joint_cb, 10)
        self.create_subscription(Twist, '/cmd_vel', self.cmd_cb, 10)
        self.pub_cmds = self.create_publisher(Float32MultiArray, '/joint_targets', 10)

        # 50Hz 정책 루프
        self.create_timer(0.02, self.tick)

    def imu_cb(self, msg): self.proj_grav[...] = [msg.linear_acceleration.z, msg.linear_acceleration.x, msg.linear_acceleration.y]
    def joint_cb(self, msg):
        self.joint_pos[...] = msg.position[:12] - np.deg2rad(90)   # default pos offset
        self.joint_vel[...] = msg.velocity[:12]
    def cmd_cb(self, msg):
        self.target_vx, self.target_vy, self.target_w = msg.linear.x, msg.linear.y, msg.angular.z

    def tick(self):
        cmd = np.array([self.target_vx, self.target_vy, self.target_w])
        gait_scale = [0.0, 1.0, 1.7][self.gait_mode]
        cmd_scaled = cmd * gait_scale

        obs = np.concatenate([self.base_lin_vel, self.base_ang_vel, self.proj_grav,
                              cmd_scaled, self.joint_pos, self.joint_vel, self.last_action])
        action = self.sess.run([self.output_name], {self.input_name: obs.astype(np.float32)[None,:]})[0][0]

        joint_targets = self.joint_default + 0.5 * action
        self.last_action = action.astype(np.float32)

        msg = Float32MultiArray()
        msg.data = joint_targets.tolist()
        self.pub_cmds.publish(msg)

4. 카메라 인지 — YOLOv8-Nano (옵션)

# file: mechadog_control/vision_yolo_node.py
import rclpy, cv2, numpy as np
from ultralytics import YOLO
from sensor_msgs.msg import Image
from std_msgs.msg import Bool
from cv_bridge import CvBridge

class VisionObstacleNode(rclpy.node.Node):
    def __init__(self):
        super().__init__('vision_obstacle')
        self.br = CvBridge()
        self.model = YOLO('yolov8n.pt')  # RKNN NPU는 변환 후 사용
        self.create_subscription(Image, '/camera/image_raw', self.cb, 10)
        self.pub = self.create_publisher(Bool, '/obstacle_ahead', 10)

    def cb(self, msg):
        img = self.br.imgmsg_to_cv2(msg, desired_encoding='bgr8')
        res = self.model.predict(img, conf=0.4, verbose=False)[0]
        has_obstacle = any(box.cls == 0 for box in res.boxes)  # person/box check
        self.pub.publish(Bool(data=has_obstacle))

5. E-stop / Failsafe 정책

# E-stop implementation in rl_policy_node
def is_safe_to_continue(self):
    if not self.rc_state.estop:
        return False, "E-stop SW OFF"
    if self.rc_state.fs_lost:
        return False, "FS link lost"
    if abs(self.base_roll) > math.radians(35) or abs(self.base_pitch) > math.radians(35):
        return False, "tilted"
    if self.battery_voltage < 9.0:    # 3S alarm
        return False, "battery low"
    return True, ""

6. 첫 실기 시연 — 권장 순서

  1. RC transmitter arm 상태(GPS 모드 off, ELRS binding OK, 8+ 채널 활성화)
  2. LiPo 연결, R24-P6 LED 점등 확인
  3. Rock 5C SSH 진입 → ros2 launch mechadog_bringup all_nodes.launch.py
  4. populate logs (TF tree, /joint_states publication OK 확인)
  5. RC 스틱 살짝 다뤄 joint_targets 반응 확인
  6. 지면에 놓기 → RC vx 살짝 + → 정책이 다리로 보행 시도
  7. 바로 옆에서 E-stop 스위치로 비상 정지 동작 확인
  8. 다시 풀고 YOLO 결과(장애물 인지의 유무) corpus 수집

7. 자주 실패하는 지점

← 하드웨어 페이지 ← RL 학습 페이지 ↩ 공식/개요