ROS1 Master 관리 프로그램

여러 ROS Master를 통합 관리하는 제어 툴

역할: 백엔드 개발자
Python ROS1 RPC API Unity(C#) UDP Socket

프로젝트 배경

문제 상황

여러 ROS Master를 동시에 운영하는 환경

  • Master A, Master B, Master C... 각각 독립 운영
  • 특정 노드를 다른 Master로 옮기고 싶을 때가 있음
  • 각 Master의 상태를 일일이 확인해야 함

불편한 점

  1. "지금 이 노드가 어느 Master에 있지?"
  2. "노드 A를 Master 1에서 Master 2로 옮기려면 설정 다시 해야 함"
  3. "여러 Master를 한눈에 볼 수 없음"

목표

만들고 싶었던 것

1. 통합 뷰

  • 모든 Master를 한 화면에서 확인
  • 각 Master의 노드 목록 표시

2. 노드 전환

  • Master A의 Node1 → Master B의 Node2로 교체
  • 드래그앤드롭으로 간단하게

3. 노드 복사

  • Master A의 노드를 Master B에도 등록
  • 원본은 유지하면서 복사

시스템 구조

전체 아키텍처

graph TD A[Unity Frontend
(사용자 UI)] -->|UDP Socket
(비동기 통신)| B[Python Backend
(Master 제어)] B -->|ROS RPC API
(xmlrpclib)| C[ROS Masters
A / B / C / ...]

계층 분리

  • Unity: 화면 표시 + 사용자 입력
  • Python: Master 제어 로직
  • ROS: 실제 로봇 시스템

주요 기능 ① Master 탐색

네트워크에서 Master 자동 찾기

사용자 동작

  1. "Master 찾기" 버튼 클릭
  2. 약 2초 대기
  3. 192.168.5.100, 101, 102... 자동 표시

내부 동작

  • 네트워크 IP 범위 스캔 (예: 192.168.5.1~254)
  • 각 IP에 ROS Master 응답 확인
  • 응답하는 IP만 리스트에 추가

멀티스레드 처리

  • 8개 스레드 병렬 실행
  • 각 스레드가 약 32개 IP 담당
  • Queue로 결과 수집

주요 기능 ② 노드 정보 수집

각 Master의 상태 가져오기

ROS Master RPC API 사용

# Master에 접속
rosobj = xmlrpclib.ServerProxy(master_uri)

# 전체 시스템 상태 요청
code, msg, systemState = rosobj.getSystemState(caller_id)

# systemState에 포함된 정보:
# - Publishers: 어떤 노드가 어떤 토픽 발행
# - Subscribers: 어떤 노드가 어떤 토픽 구독

수집 정보

  • 노드 목록: /camera_node, /control_node...
  • 토픽 목록: /image, /cmd_vel...
  • 토픽 타입: sensor_msgs/Image...
  • Publisher-Subscriber 관계

주요 기능 ③ 노드 전환

Master 간 노드 이동

시나리오

Master A: old_node (기존) Master B: new_node (새 버전) → Master A의 old_node를 new_node로 교체하고 싶음

UI 동작

  1. Master B의 new_node를 마우스로 드래그
  2. Master A의 old_node 위에 드롭
  3. 0.1초 후 교체 완료

결과

  • Master A: new_node로 교체됨
  • Master B: new_node 삭제됨
  • 토픽 연결 자동으로 재구성

노드 전환 - 내부 로직

4단계 프로세스

Step 1: 검증

# 두 노드가 같은 토픽을 사용하는지 확인
if beforeNode.pub_topic != afterNode.pub_topic:
    return False  # 발행 토픽 다름
if beforeNode.sub_topic != afterNode.sub_topic:
    return False  # 구독 토픽 다름

Step 2: 새 노드 등록

# Master A에 new_node 등록
for topic in afterNode.pub_topic:
    beforeNodeMaster.registerPublisher(...)
for topic in afterNode.sub_topic:
    beforeNodeMaster.registerSubscriber(...)

Step 3: 기존 노드 제거

# Master A에서 old_node 제거
for topic in beforeNode.pub_topic:
    beforeNodeMaster.unregisterPublisher(...)
beforeNode.stop()  # 프로세스 종료

Step 4: 연결 업데이트

# Subscriber들에게 Publisher 변경 알림
afterNode.publisherUpdate(topic, new_publishers)

노드 전환 - 핵심 아이디어

ROS는 노드 이동 기능이 없음

일반적인 ROS 사용

import rospy
rospy.init_node('my_node')
# 고수준 API - 편하지만 Master 제어 불가

이 프로젝트 접근

import xmlrpclib
rosobj = xmlrpclib.ServerProxy(master_uri)
rosobj.registerPublisher(...)
# 저수준 RPC API - Master 직접 제어 가능

발견한 핵심

  • publisherUpdate() 메서드 존재
  • Subscriber에게 강제로 새 Publisher 알릴 수 있음
  • 이걸로 무중단 전환 구현

주요 기능 ④ 노드 복사

하나의 노드를 여러 Master에

동작

Master A: lidar_node (원본) → Master B의 빈 슬롯에 드래그 결과: Master A: lidar_node (유지) Master B: lidar_node (복사)

구현

def Copy_Node(target_master, target_node):
    # target_master에 등록만 함
    RegistNode(target_master, target_node)
    # Publisher 정보 업데이트
    DiffApply(target_master, target_node)
    # 원본 노드는 그대로

활용

  • 하나의 센서 데이터를 여러 곳에서 사용
  • 각 Master가 독립적으로 처리

Unity UI

직관적인 인터페이스

화면 레이아웃

Unity UI 레이아웃

드래그앤드롭

  • 노드를 클릭해서 드래그
  • 다른 Master의 슬롯에 드롭
  • 자동으로 전환/복사 처리

시각적 피드백

  • 흰색 슬롯: 빈 칸
  • 파란색: 복사된 노드
  • 보라색: 교체된 노드

Unity 구현

드래그 이벤트 처리

RosNode.cs

// 드래그 시작
public void OnBeginDrag(PointerEventData eventData) {
    clone = Instantiate(gameObject);  // 복제본 생성
    clone.transform.SetParent(canvas.transform);
}

// 드래그 중
public void OnDrag(PointerEventData eventData) {
    clone.transform.position = eventData.position;  // 마우스 따라감
}

// 드롭
public void OnEndDrag(PointerEventData eventData) {
    var slot = hit.GetComponent<RosSlot>();
    if (slot.SlotFilled) {
        // 기존 노드 있음 → changeNode
    } else {
        // 빈 슬롯 → copyNode
    }
}

통신 프로토콜

Unity ↔ Python

UDP Socket 사용 이유

  • 가벼움 (TCP보다 오버헤드 적음)
  • 명령-응답 구조에 적합
  • 로컬 환경이라 패킷 손실 없음

메시지 형식

# 노드 전환 요청
Unity → Python:
"changeNode/old_node/new_node/192.168.5.100/192.168.5.101"

Python → Unity:
"clear!!" (성공) 또는 "diff!!" (실패)

# 노드 복사 요청
Unity → Python:
"copy_node/192.168.5.100/192.168.5.101/node_name"

비동기 처리

  • Unity: 별도 스레드에서 UDP 처리
  • Python: 메인 루프에서 명령 대기
  • Queue로 스레드 간 데이터 전달

Python 백엔드

Receiver.py 구조

UDP 서버

class DEReceiver:
    def __init__(self, ip, port):
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    
    def act(self):
        # Unity에서 명령 수신
        data, addr = self.sock.recvfrom(1024)
        func = data.split('/')
        
        # 해당 함수 실행
        result = getattr(self, func[0])(func)
        
        # 결과 전송
        if result != "":
            self.sock.sendto(result, addr)

명령 처리

  • check_master_ip: Master 탐색
  • changeNode: 노드 전환
  • copy_node: 노드 복사

기술적 도전 ①

Publisher-Subscriber 연결 끊김 문제

문제 상황

Master A: old_node (pub) → Subscriber Master A: new_node (pub)로 교체 → Subscriber가 데이터 못 받음

원인

  • Subscriber는 old_node만 알고 있음
  • new_node로 자동 연결 안 됨

해결: DiffApply 함수

def DiffApply(target_master, target_node):
    # 1. 새 Publisher URI 리스트 생성
    publishers = [node.node_uri for node in pub_nodes]
    
    # 2. Subscriber에게 강제 알림
    target_node.publisherUpdate(topic, publishers)
    
    # 3. Publisher도 준비시키기
    for pub_node in publishers:
        pub_node.requestTopic(topic)

기술적 도전 ②

Unity 메인 스레드 제약

문제

  • UDP 수신이 블로킹됨 → 화면 멈춤
  • Unity는 메인 스레드만 UI 접근 가능

해결

// 별도 스레드: UDP 처리
private void RunClientThread() {
    while (loop) {
        received = udpClient.Receive(ref srv);
        srv_q.Enqueue(received);  // Queue에 저장
    }
}

// 메인 스레드: UI 업데이트
void Update() {
    if (srv_q.Count() > 0) {
        string data = srv_q.Dequeue();
        UpdateUI(data);  // 여기서만 UI 조작
    }
}

Queue 사용

  • 스레드 안전한 데이터 전달
  • 메인 스레드는 블로킹 없이 UI 처리

기술적 도전 ③

일부 Subscriber만 연결 성공

증상

  • 노드 전환 후 Subscriber A는 OK
  • Subscriber B는 타임아웃

디버깅

# 처음 시도
def DiffApply(target_master, target_node):
    target_node.publisherUpdate(topic, publishers)
    # 이것만 하면 일부만 성공

# 개선
def DiffApply(target_master, target_node):
    target_node.publisherUpdate(topic, publishers)
    
    # 이거 추가하니까 100% 성공
    for pub_node in publishers:
        pub_node.requestTopic(topic)

원인

  • publisherUpdate()는 알리기만 함
  • requestTopic()으로 TCPROS 준비 필요

성과

실제 효과

작업 시간

  • 노드 전환: 수동 설정 → 드래그 0.1초
  • Master 탐색: 일일이 입력 → 자동 2초

오류 감소

  • 설정 오류: 자주 발생 → 없음
  • 토픽 연결 실패: 가끔 → 없음

사용성

  • 직관적인 UI
  • 학습 시간 거의 없음
  • 실시간 상태 확인

한계점

아쉬웠던 부분

1. ROS1 전용

  • ROS2는 구조가 다름 (Master 없음)
  • 이 방식으로는 못 함

2. 모니터링 부족

  • 노드 상태만 표시
  • 실제 토픽 데이터는 안 보임
  • CPU/메모리 사용량 없음

3. 로컬 전용

  • 같은 네트워크에서만 사용
  • 외부 접근 불가

배운 점

기술적 성장

ROS 내부 구조 이해

  • Master RPC API 직접 사용
  • TCPROS 프로토콜 이해
  • Publisher-Subscriber 메커니즘

분산 시스템 경험

  • 여러 Master 상태 동기화
  • 네트워크 통신 설계
  • 에러 처리 전략

풀스택 개발

  • Python 백엔드
  • Unity 프론트엔드
  • UDP 통신 연결

문제 해결 능력

문서 없이 해결하기

상황

  • ROS Master API 문서: 기본 설명만
  • 노드 이동 예제: 전혀 없음
  • 가능한지조차 모름

접근 방법

  1. ROS 소스코드 분석
  2. 다양한 API 조합 실험
  3. publisherUpdate() 발견
  4. 추가로 requestTopic() 필요 파악

결과

  • 문서에 없는 기능 구현
  • 깊은 이해 획득

마무리

프로젝트 요약

만든 것

  • 여러 ROS Master 통합 관리 툴
  • 드래그앤드롭 노드 전환/복사

핵심 기술

  • ROS Master RPC API 직접 제어
  • Unity-Python UDP 통신
  • 멀티스레드 네트워크 스캔

성과

  • 작업 시간 대폭 단축
  • 설정 오류 제거
  • 직관적인 UI

배운 것

  • 분산 시스템 설계
  • 문서 없이 문제 해결
  • 풀스택 개발 경험

시연 영상

프로젝트 시연 영상

1 / 22