arm_sdk_publisher.Write(self.low_cmd)가 계속 호출되고 있다면, 설령 특정 플래그를 0으로 보낸다 하더라도 low_cmd 내의 다른 관절값(q)들이 여전히 MoveIt에서 받은 마지막 상태를 유지하며 로봇 컨트롤러에 "위치 명령"으로 전달되고 있을 것입니다. 즉, G1ArmActionClient가 보내는 명령과 arm_sdk_publisher가 보내는 명령이 로봇의 모터 제어기 레벨에서 **경합(Conflict)**을 일으키는 것입니다.
이를 완벽하게 Pause 하기 위한 가장 확실한 방법 3가지를 제안해 드립니다.
방법 1: Write 자체를 중단하고 토픽 발행 중지 (가장 추천)
단순히 특정 값을 0으로 만드는 것이 아니라, paused 상태일 때는 arm_sdk_publisher.Write 호출 자체를 건너뛰어야 합니다. 하지만 Unitree SDK 구조상 연결을 유지해야 할 수도 있으므로, 제어권 전환 시에는 메시지 전송을 멈추는 것이 핵심입니다.
def LowCmdWrite(self):
self.time_ += self.control_dt_
if self.paused:
# 1. 아예 Write를 호출하지 않음으로써 SDK 수준의 명령 충돌 방지
# 2. 로봇 내장 컨트롤러가 자유롭게 제어할 수 있도록 제어권을 넘김
return # 함수를 여기서 종료하여 하단 Write 로직 실행 방지
# ... 기존의 q값 계산 및 Write 로직 ...
self.low_cmd.crc = self.crc.Crc(self.low_cmd)
self.arm_sdk_publisher.Write(self.low_cmd)
방법 2: ROS Service를 이용한 동적 Pause (런타임 제어)
face_recognition_node_v2.py에서 동작을 호출하기 직전에 g1_29dof_arm7_ys_waist.py 노드에 "잠시 멈춰(Pause)"라고 명령을 내리는 서비스 서버를 구현하는 방법입니다.
g1_29dof_arm7_ys_waist.py 내부에 추가:
from std_srvs.srv import SetBool
# ... __init__ 내부 ...
self.pause_service = rospy.Service('pause_moveit_stream', SetBool, self.handle_pause)
def handle_pause(self, req):
self.paused = req.data
status = "Paused" if self.paused else "Resumed"
return {'success': True, 'message': f"MoveIt stream {status}"}
face_recognition_node_v2.py에서 호출:
def call_g1_action(self):
# 1. MoveIt 스트리밍 일시 정지 요청
rospy.wait_for_service('pause_moveit_stream')
pause_proxy = rospy.ServiceProxy('pause_moveit_stream', SetBool)
pause_proxy(True)
# 2. G1ArmActionClient 동작 수행
self.arm_client.greet()
# 3. 동작 완료 후 다시 MoveIt 스트리밍 재개
pause_proxy(False)
방법 3: 가속도/속도 값이 0인 “Null 명령” 전달 (SDK 제약 시)
만약 SDK 통신 세션이 끊기면 안 되는 구조라면, paused일 때 모든 관절의 kp (Stiffness)와 kd (Damping) 값을 0으로 만들어 로봇이 MoveIt의 명령을 무시하고 외부 명령(G1ArmActionClient)에 순응하게 만들 수 있습니다.
if self.paused:
for motor in self.low_cmd.motor_cmd:
motor.q = 0 # 의미 없음
motor.kp = 0 # 제어 강도를 0으로 만들어 명령 무시
motor.kd = 0
motor.tau = 0
self.low_cmd.crc = self.crc.Crc(self.low_cmd)
self.arm_sdk_publisher.Write(self.low_cmd)
return
요약 및 조언
현재 오동작이 발생하는 주된 이유는 LowCmdWrite가 루프를 돌며 계속 low_cmd를 쓰고 있기 때문입니다.
**방법 2(ROS Service)**를 사용해 얼굴 인식 노드와 제어 노드 간에 명확한 '제어권 전환 시그널’을 주고받는 것이 가장 깔끔하며, 이때 paused 로직 안에서는 방법 1처럼 Write를 아예 수행하지 않거나 방법 3처럼 게인(kp, kd)을 0으로 설정하는 것을 권장합니다.
이렇게 하면 G1ArmActionClient가 동작하는 동안 MoveIt의 데이터가 로봇으로 전달되는 것을 물리적으로 차단할 수 있습니다.