High Level Command — C++ 예제

소스: rbq_sdk/cpp/example/src/rbq_high_level.cpp

DDS를 통해 HighLevelCommand 메시지를 50 Hz로 발행합니다.
보행 선택은 터미널 입력으로, 속도 setpoint는 조이스틱으로 제어합니다.

개요

세 가지 동시 실행 구성요소:

구성요소	주기	역할
제어 루프 스레드	50 Hz	현재 보행 + 속도로 HighLevelCommand 발행
조이스틱 루프 스레드	100 Hz	조이스틱 폴링 후 속도 전역 변수 업데이트
메인 스레드 (터미널)	블로킹	stdin에서 보행 이름/ID를 읽어 목표 보행 업데이트

시작 시 제어 루프가 switchControlMode = true를 한 번 발행하여 로봇 스택에서 EXT_JOY 모드를 활성화합니다 — 이 토픽이 발행되지 않으면 HighLevelCommand 토픽은 무시됩니다.

사전 요구사항

• RBQ SDK 빌드 및 설치 완료 — C++ SDK 개요 참조.
• Linux 조이스틱 장치 /dev/input/js* (조이스틱이 필수).
• 로봇 스택이 실행 중(시뮬레이션 또는 하드웨어).

빌드

bash scripts/setup.bash
bash scripts/build.bash

바이너리: bin/rbq_high_level

실행

./bin/rbq_high_level [networkInterface] [joystickPath]

# 시뮬레이터 / 로봇 내부 스택 — 인터페이스 기본값은 lo
./bin/rbq_high_level

# 실제 로봇 — 로봇에 닿는 인터페이스를 넘깁니다
./bin/rbq_high_level eth0
./bin/rbq_high_level eth0 /dev/input/js1

조이스틱 자동 감지는 Sport Client (조이스틱) 예제와 동일한 로직을 따릅니다.

터미널 명령

> 프롬프트에서 보행 이름 또는 숫자 보행 ID를 입력합니다.

list              # 예제 내장 목록 출력
standing          # STANDING으로 전환 (이름으로)
3                 # TROTTING으로 전환 (ID로)
waving            # WAVING으로 전환 (이름으로)

지원 보행 목록

이름	ID	설명
sitting	0	지면에 앉기
standing	1	서기 자세 (시작 시 기본값)
trotting	3	MPC 트롯 보행
trot_stairs	4	계단 적응형 트롯
waving	5	느린 걷기 보행
docking	10	자동 도킹 시퀀스

예제 내장 목록에는 지원하지 않는 내부 보행도 함께 출력됩니다. 위 표의 보행만 사용하세요.

docking(10)을 명령한 뒤에는 RobotStatus의 docking_state 필드로 진행 상태를 지켜보세요 — 단계별 진행과 중단 시의 실패 사유가 여기로 보고됩니다. 도킹 상태 표를 참고하세요.

조이스틱 속도 제어

축	매핑	최대값
왼쪽 스틱 Y (앞으로)	vel_x	±1.2 m/s
왼쪽 스틱 X (왼쪽으로)	vel_y	±0.5 m/s
오른쪽 스틱 X (요)	omega_z	±60 deg/s

데드존: 0.12 적용 후 스케일링.

omega_z 단위 주의: HighLevelCommand.omega_z는 도/초(deg/s) 단위로 전송됩니다.
로봇 내부 QuadWalk가 D2R을 곱해 rad/s로 변환합니다 (예: 60 deg/s ≈ 1.05 rad/s 요 속도).

사용되는 DDS 토픽

방향	토픽	메시지 타입
발행	rt/rbq/cmd/high_level	rbq_msgs::msg::dds_::HighLevelCommand_
발행	rt/rbq/cmd/switch_control_mode	std_msgs::msg::dds_::Bool_

주요 구현 사항

• gait_transition 플래그: 보행 변경 직후 단 한 틱에만 true로 설정되며, std::atomic<bool>::exchange()로 정확히 한 번만 소비됩니다. QuadWalk gaitTimer가 연속으로 트리거되는 것을 방지합니다.
• switchControlMode: 시작 시 200 ms 딜레이 후 한 번 발행됩니다 (DDS 구독자 연결 대기). 발행하지 않으면 로봇은 조이스틱 모드를 유지하고 HighLevelCommand를 무시합니다.
• 타이밍: 제어 루프는 clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, ...)을 사용해 정밀한 20 ms 스케줄링을 구현합니다.

관련 문서

• Sport Client
• Sport Client (조이스틱)
• Low Level (RL 정책)
• C++ SDK 개요