SimpleWaypointFollower에 로봇 상태 → RGB LED 색상 변환 기능 추가

1. 개요

로봇을 실제 현장에서 운용하다 보면 터미널 로그만으로는 현재 상태를 바로 파악하기 어렵습니다.

예를 들어 로봇이 다음 중 어떤 상태인지 즉시 알아야 합니다.

부팅 중인지
Nav2가 준비 중인지
주행 중인지
경유점에 도착했는지
미션이 성공했는지
실패했는지
사용자가 중지했는지

이런 상태를 RGB LED 색상으로 표시하면 현장에서 로봇 상태를 훨씬 직관적으로 확인할 수 있습니다.

이번 글에서는 기존의 두 가지 기능을 결합합니다.

첫 번째는 /rgb_led/cmd 토픽으로 들어온 RGB LED 메시지를 시리얼 명령으로 변환해 아두이노 또는 LED 컨트롤러로 보내는 노드입니다.

두 번째는 Nav2 BasicNavigator를 사용해 경유점 주행을 수행하고, 주행 상황에 따라 음성 안내를 출력하는 노드입니다.

여기에 로봇 상태 → RGB LED 색상 변환 기능을 추가해 다음 구조를 만듭니다.

핵심은 주행 노드가 LED 하드웨어를 직접 제어하지 않는다는 점입니다.

주행 노드는 단지 /rgb_led/cmd 토픽으로 RGB 메시지만 발행합니다. 실제 시리얼 통신은 기존 RGB LED 시리얼 노드가 담당합니다.

이 구조가 깔끔합니다. 유지보수도 쉽고, LED 제어 장치가 바뀌어도 주행 로직은 거의 그대로 사용할 수 있습니다.

2. 상태별 RGB LED 색상 정책

이번 예제에서는 로봇 상태에 따라 다음과 같이 LED 색상을 지정했습니다.

로봇 상태	의미	RGB 색상
BOOTING	노드 시작	흰색
NAV2_WAITING	Nav2 활성화 대기	주황색
READY	주행 준비 완료	파란색
MISSION_START	미션 시작	청록색
MOVING	경유점 이동 중	초록색
WAYPOINT_REACHED	경유점 도착 표시	보라색
SUCCEEDED	미션 성공	초록색
CANCELED	미션 취소	노란색
FAILED	미션 실패	빨간색
EMERGENCY_STOP	사용자 중지 / 긴급 정지	빨간색
IDLE	종료 / 대기	LED 꺼짐

실제 로봇에서는 색상 정책이 중요합니다.

일반적으로 다음과 같이 잡는 것이 직관적입니다.

초록색: 정상
파란색: 준비
노란색: 주의
빨간색: 실패 또는 긴급 상황
흰색: 부팅
꺼짐: 종료 또는 대기

현장 작업자는 색상만 보고도 로봇의 상태를 즉시 판단할 수 있습니다.

3. 전체 Python 소스

파일명 예시:

robot_state_led_waypoint_follower.py

기존 simple_waypoint_follower.py에 기능을 추가한 것입니다.

전체 소스는 다음과 같습니다.

#!/usr/bin/env python3

import math
import time
from enum import Enum

import rclpy
from rclpy.node import Node
from pathlib import Path
import yaml

from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult

from robot_audio_interfaces.msg import AudioCommand
from rgb_led_interfaces.msg import RgbLed
from sensor_msgs.msg import BatteryState


class RobotState(Enum):
    """Robot state definitions mapped to RGB LED colors."""

    BOOTING = 'booting'
    NAV2_WAITING = 'nav2_waiting'
    READY = 'ready'
    MISSION_START = 'mission_start'
    MOVING = 'moving'
    WAYPOINT_REACHED = 'waypoint_reached'
    SUCCEEDED = 'succeeded'
    CANCELED = 'canceled'
    FAILED = 'failed'
    EMERGENCY_STOP = 'emergency_stop'
    IDLE = 'idle'


class RobotStateLedWaypointFollower(Node):
    
    def __init__(self):
        super().__init__('robot_state_led_waypoint_follower')

        self.declare_parameter('led_topic', '/rgb_led/cmd')
        self.declare_parameter('audio_topic', '/audio/command')
        self.declare_parameter('feedback_period_sec', 0.5)
        self.declare_parameter('waypoint_reached_flash_sec', 0.3)
        self.declare_parameter('wp_file', 'waypoints.yaml')

        self.led_topic = self.get_parameter('led_topic').value
        self.audio_topic = self.get_parameter('audio_topic').value
        self.wp_file = self.get_parameter('wp_file').value
        self.feedback_period_sec = float(self.get_parameter('feedback_period_sec').value)
        self.waypoint_reached_flash_sec = float(
            self.get_parameter('waypoint_reached_flash_sec').value
        )

        self.led_pub = self.create_publisher(RgbLed, self.led_topic, 10)
        self.audio_pub = self.create_publisher(AudioCommand, self.audio_topic, 10)

        self.create_subscription(
            BatteryState,
            "/battery_state",
            self.battery_state_callback,
            10
        )

        self.bat_soc = 0.0
        self.voltage = 0.0
        self.current_time = 0
        self.prev_time = 0

        self.navigator = BasicNavigator()

        self.yaml_data = self.load_waypoint_yaml(self.wp_file)

        self.frame_id = self.yaml_data.get("frame_id", "map")

        self.state_color_map = {
            RobotState.BOOTING: (255, 255, 255),         # white
            RobotState.NAV2_WAITING: (255, 180, 0),     # orange
            RobotState.READY: (0, 0, 255),              # blue
            RobotState.MISSION_START: (0, 255, 255),    # cyan
            RobotState.MOVING: (0, 255, 0),             # green
            RobotState.WAYPOINT_REACHED: (180, 0, 255), # purple
            RobotState.SUCCEEDED: (0, 255, 0),          # green
            RobotState.CANCELED: (255, 255, 0),         # yellow
            RobotState.FAILED: (255, 0, 0),             # red
            RobotState.EMERGENCY_STOP: (255, 0, 0),     # red
            RobotState.IDLE: (0, 0, 0),                 # off
        }

        self.current_state = None
        self.set_robot_state(RobotState.BOOTING)

        self.get_logger().info('Robot State LED Waypoint Follower initialized')
        self.get_logger().info(f'LED topic: {self.led_topic}')
        self.get_logger().info(f'Audio topic: {self.audio_topic}')

    def battery_state_callback(self, msg):
        self.bat_soc = msg.percentage
        self.voltage = msg.voltage

    def yaw_to_quaternion(self, yaw: float):        
        qz = math.sin(yaw * 0.5)
        qw = math.cos(yaw * 0.5)
        return qz, qw
    
    def load_waypoint_yaml(self, yaml_path: str) -> dict:
        path = Path(yaml_path).expanduser()

        if not path.exists():
            raise FileNotFoundError(f"YAML file not found: {path}")

        with open(path, "r", encoding="utf-8") as file:
            data = yaml.safe_load(file)

        if data is None:
            raise ValueError("YAML file is empty.")

        if "waypoints" not in data:
            raise ValueError("YAML file must contain 'waypoints' field.")

        if not isinstance(data["waypoints"], list):
            raise ValueError("'waypoints' must be a list.")

        return data    


    def create_pose(self, x, y, yaw):
        pose = PoseStamped()
        pose.header.frame_id = 'map'
        pose.header.stamp = self.get_clock().now().to_msg()

        pose.pose.position.x = float(x)
        pose.pose.position.y = float(y)
        pose.pose.position.z = 0.0

        pose.pose.orientation.z = math.sin(yaw / 2.0)
        pose.pose.orientation.w = math.cos(yaw / 2.0)

        return pose

    def clamp_u8(self, value):
        return max(0, min(255, int(value)))

    def publish_led_rgb(self, r, g, b, enable=True):
        msg = RgbLed()
        msg.enable = bool(enable)
        msg.r = self.clamp_u8(r)
        msg.g = self.clamp_u8(g)
        msg.b = self.clamp_u8(b)

        self.led_pub.publish(msg)

    def set_robot_state(self, state):
        if state not in self.state_color_map:
            self.get_logger().warn(f'Unknown robot state: {state}')
            return

        self.current_state = state

        r, g, b = self.state_color_map[state]
        self.publish_led_rgb(r, g, b, enable=True)

        self.get_logger().info(
            f'Robot state changed: {state.value} -> RGB({r}, {g}, {b})'
        )

    def flash_state(self, state, return_state=None, duration_sec=0.3):
        if return_state is None:
            return_state = self.current_state

        self.set_robot_state(state)
        time.sleep(duration_sec)

        if return_state is not None:
            self.set_robot_state(return_state)

    def publish_audio(self, text='', sound_id='', audio_type=AudioCommand.TYPE_TTS):
        msg = AudioCommand()
        msg.type = audio_type
        msg.text = text
        msg.sound_id = sound_id
        msg.volume = 1.0
        msg.repeat = 1

        self.audio_pub.publish(msg)

    def speak(self, text):
        self.publish_audio(
            text=text,
            sound_id='',
            audio_type=AudioCommand.TYPE_TTS
        )

    def play_effect(self, sound_id):
        self.publish_audio(
            text='',
            sound_id=sound_id,
            audio_type=AudioCommand.TYPE_EFFECT
        )

    def speak_with_effect(self, text, sound_id):
        self.publish_audio(
            text=text,
            sound_id=sound_id,
            audio_type=AudioCommand.TYPE_TTS_AND_EFFECT
        )

    def run(self):
        self.get_logger().info('Waiting for Nav2 to become active...')

        self.set_robot_state(RobotState.NAV2_WAITING)
        self.speak('내비게이션 시스템을 준비합니다')

        self.navigator.waitUntilNav2Active()

        self.get_logger().info('Nav2 is active')

        self.set_robot_state(RobotState.READY)
        self.speak_with_effect('경유점 주행을 시작합니다', 'start')

        time.sleep(1.0)

        self.set_robot_state(RobotState.MISSION_START)
        time.sleep(0.5)

        goal_poses = []

        for wp in self.yaml_data["waypoints"]:
            name = wp.get("name", "noname")
            x = wp["x"]
            y = wp["y"]
            yaw = wp.get("yaw", 0.0)

            pose = self.create_pose(
                x=x,
                y=y,
                yaw=yaw,
            )

            goal_poses.append(pose)
            self.get_logger().info(f"Loaded waypoint: {name}, x={x}, y={y}, yaw={yaw}")


        self.navigator.followWaypoints(goal_poses)

        self.set_robot_state(RobotState.MOVING)

        last_feedback_index = -1

        while not self.navigator.isTaskComplete():
            rclpy.spin_once(self, timeout_sec=0.0)

            self.current_time = time.time()
            if self.current_time - self.prev_time > 30.0:
                batinfo_msg = f'배터리 전압은 {int(self.voltage)}볼트이고, 충전량은 {int(self.bat_soc)} 퍼센트 입니다'
                # batinfo_msg = f'배터리 충전량은 {int(self.bat_soc)} 퍼센트 입니다'
                self.get_logger().info(batinfo_msg)
                self.speak(batinfo_msg)
                self.prev_time = self.current_time

            feedback = self.navigator.getFeedback()

            if feedback is not None:
                current_index = feedback.current_waypoint

                if current_index != last_feedback_index:
                    if last_feedback_index != -1:
                        self.flash_state(
                            RobotState.WAYPOINT_REACHED,
                            return_state=RobotState.MOVING,
                            duration_sec=self.waypoint_reached_flash_sec,
                        )

                    last_feedback_index = current_index

                    msg = f'{current_index + 1}번 경유점으로 이동 중입니다'
                    self.get_logger().info(msg)
                    self.speak(msg)

            time.sleep(self.feedback_period_sec)

        result = self.navigator.getResult()

        if result == TaskResult.SUCCEEDED:
            self.set_robot_state(RobotState.SUCCEEDED)
            self.get_logger().info('Waypoint mission succeeded')
            self.speak_with_effect('모든 경유점 주행을 완료했습니다', 'goal')

        elif result == TaskResult.CANCELED:
            self.set_robot_state(RobotState.CANCELED)
            self.get_logger().warn('Waypoint mission was canceled')
            self.speak_with_effect('경유점 주행이 취소되었습니다', 'warning')

        elif result == TaskResult.FAILED:
            self.set_robot_state(RobotState.FAILED)
            self.get_logger().error('Waypoint mission failed')
            self.speak_with_effect('경유점 주행에 실패했습니다', 'error')

        else:
            self.set_robot_state(RobotState.CANCELED)
            self.get_logger().warn('Unknown waypoint result')
            self.speak_with_effect('알 수 없는 주행 결과입니다', 'warning')

    def destroy_node(self):
        try:
            self.set_robot_state(RobotState.IDLE)
        except Exception:
            pass

        super().destroy_node()


def main(args=None):
    rclpy.init(args=args)

    node = RobotStateLedWaypointFollower()

    try:
        node.run()

    except KeyboardInterrupt:
        node.get_logger().warn('Keyboard interrupt received')
        node.set_robot_state(RobotState.EMERGENCY_STOP)
        node.speak_with_effect('사용자에 의해 주행이 중지되었습니다', 'warning')
        time.sleep(0.5)

    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

4. 소스 설명

1) 로봇 상태 Enum 추가

새 소스에서 가장 먼저 눈에 띄는 추가 내용은 RobotState Enum입니다.

class RobotState(Enum):
    BOOTING = 'booting'
    NAV2_WAITING = 'nav2_waiting'
    READY = 'ready'
    MISSION_START = 'mission_start'
    MOVING = 'moving'
    WAYPOINT_REACHED = 'waypoint_reached'
    SUCCEEDED = 'succeeded'
    CANCELED = 'canceled'
    FAILED = 'failed'
    EMERGENCY_STOP = 'emergency_stop'
    IDLE = 'idle'

기존 소스에서는 로봇의 상태를 별도 상태값으로 관리하지 않았습니다. 로그와 음성 안내만으로 현재 상황을 표현했습니다. 예를 들어 Nav2 준비, 주행 시작, 성공, 실패 등을 각각 로그와 TTS로 출력하는 방식이었습니다.

새 소스는 상태를 명확한 Enum으로 정의했습니다. 이 덕분에 로봇의 동작 흐름을 다음처럼 체계적으로 관리할 수 있습니다.

BOOTING
NAV2_WAITING
READY
MISSION_START
MOVING
WAYPOINT_REACHED
SUCCEEDED
CANCELED
FAILED
EMERGENCY_STOP
IDLE

이 방식은 LED 색상, 음성 안내, 로그 출력을 상태 기준으로 묶기 좋습니다. 즉, 새 소스는 단순히 “주행한다”에서 끝나는 코드가 아니라, “현재 로봇이 어떤 상태인지 표현하는 코드”로 확장되었습니다.

2) RGB LED 제어 기능 추가

기존 소스에는 오디오 publisher는 있었지만 RGB LED publisher는 없었습니다.

기존 소스:

self.audio_pub = self.create_publisher(
    AudioCommand,
    '/audio/command',
    10
)

기존 소스는 음성 안내 중심이었습니다.

새 소스에는 RGB LED 메시지 타입과 publisher가 추가되었습니다.

from rgb_led_interfaces.msg import RgbLed

self.led_pub = self.create_publisher(RgbLed, self.led_topic, 10)

이제 로봇은 음성뿐만 아니라 LED 색상으로도 상태를 표현할 수 있습니다.

LED 명령을 실제로 보내는 함수도 새로 추가되었습니다.

def publish_led_rgb(self, r, g, b, enable=True):
    msg = RgbLed()
    msg.enable = bool(enable)
    msg.r = self.clamp_u8(r)
    msg.g = self.clamp_u8(g)
    msg.b = self.clamp_u8(b)

    self.led_pub.publish(msg)

이 함수는 RGB 값을 받아 /rgb_led/cmd 같은 LED 제어 토픽으로 메시지를 발행합니다.

3) LED 값 보호용 `clamp_u8()` 추가

새 소스에는 RGB 값이 0~255 범위를 벗어나지 않도록 제한하는 함수가 추가되었습니다.

def clamp_u8(self, value):
    return max(0, min(255, int(value)))

RGB LED 값은 일반적으로 0부터 255 사이의 정수값을 사용합니다. 이 함수는 잘못된 값이 들어오더라도 안전한 범위로 보정합니다.

예를 들면 다음과 같습니다.


-20	0
100	100
300	255

이런 보호 처리는 하드웨어 제어 코드에서 중요합니다. LED 제어 메시지에 이상한 값이 들어가는 것을 막아주기 때문입니다.

4) 상태별 LED 색상 매핑 추가

새 소스에는 로봇 상태와 LED 색상을 연결하는 state_color_map이 추가되었습니다.

self.state_color_map = {
    RobotState.BOOTING: (255, 255, 255),
    RobotState.NAV2_WAITING: (255, 180, 0),
    RobotState.READY: (0, 0, 255),
    RobotState.MISSION_START: (0, 255, 255),
    RobotState.MOVING: (0, 255, 0),
    RobotState.WAYPOINT_REACHED: (180, 0, 255),
    RobotState.SUCCEEDED: (0, 255, 0),
    RobotState.CANCELED: (255, 255, 0),
    RobotState.FAILED: (255, 0, 0),
    RobotState.EMERGENCY_STOP: (255, 0, 0),
    RobotState.IDLE: (0, 0, 0),
}

기존 소스에서는 상태별 시각 표시가 없었습니다. 새 소스에서는 상태가 바뀔 때마다 LED 색상이 함께 바뀝니다.

상태별 의미는 앞에 정의한 표를 참조해 주시기 바랍니다.

이 추가 기능의 핵심은 터미널을 보지 않아도 로봇 상태를 LED로 바로 확인할 수 있다는 점입니다.

5) `set_robot_state()` 함수 추가

상태 변경과 LED 출력을 한 번에 처리하는 함수가 추가되었습니다.

def set_robot_state(self, state):
    if state not in self.state_color_map:
        self.get_logger().warn(f'Unknown robot state: {state}')
        return

    self.current_state = state

    r, g, b = self.state_color_map[state]
    self.publish_led_rgb(r, g, b, enable=True)

    self.get_logger().info(
        f'Robot state changed: {state.value} -> RGB({r}, {g}, {b})'
    )

이 함수는 다음 작업을 수행합니다.

입력된 상태가 유효한 상태인지 검사
현재 상태값 갱신
상태에 맞는 RGB 색상 선택
LED 토픽으로 색상 메시지 발행
상태 변경 로그 출력

기존 소스에서는 상태 변경이라는 개념이 없었기 때문에, 각 상황에서 로그와 음성만 직접 출력했습니다. 새 소스에서는 상태를 중심으로 LED까지 함께 제어합니다.

예를 들어 아래 한 줄이면:

self.set_robot_state(RobotState.MOVING)

내부적으로 초록색 LED가 켜지고, 현재 상태가 MOVING으로 저장됩니다.

6) 경유점 도달 LED flash 기능 추가

새 소스에는 flash_state() 함수가 추가되었습니다.

def flash_state(self, state, return_state=None, duration_sec=0.3):
    if return_state is None:
        return_state = self.current_state

    self.set_robot_state(state)
    time.sleep(duration_sec)

    if return_state is not None:
        self.set_robot_state(return_state)

이 함수는 특정 상태의 LED를 잠깐 표시한 뒤 원래 상태로 되돌리는 기능입니다.

새 소스에서는 경유점에 도달했다고 판단될 때 이 함수를 사용합니다.

self.flash_state(
    RobotState.WAYPOINT_REACHED,
    return_state=RobotState.MOVING,
    duration_sec=self.waypoint_reached_flash_sec,
)

동작은 다음과 같습니다.

로봇이 이동 중이면 초록색 LED 표시
경유점 도달 감지
보라색 LED를 짧게 표시
다시 이동 중 상태인 초록색 LED로 복귀

기존 소스에서는 경유점이 바뀌면 음성으로만 안내했습니다.

msg = f'{current_index + 1}번 경유점으로 이동 중입니다'
self.get_logger().info(msg)
self.speak(msg)

새 소스는 여기에 LED flash를 추가해서 경유점 통과 이벤트를 시각적으로도 표시합니다.

7) ROS 2 파라미터 추가

기존 소스는 주요 설정값이 코드에 직접 고정되어 있었습니다.

예를 들어 오디오 토픽은 /audio/command로 고정되어 있고, 경유점도 코드 내부 리스트로 고정되어 있습니다.

새 소스는 다음 파라미터들을 선언합니다.

self.declare_parameter('led_topic', '/rgb_led/cmd')
self.declare_parameter('audio_topic', '/audio/command')
self.declare_parameter('feedback_period_sec', 0.5)
self.declare_parameter('waypoint_reached_flash_sec', 0.3)
self.declare_parameter('wp_file', 'waypoints.yaml')

각 파라미터의 의미는 다음과 같습니다.

파라미터	의미
`led_topic`	RGB LED 명령 토픽
`audio_topic`	오디오 명령 토픽
`feedback_period_sec`	Nav2 feedback 확인 주기
`waypoint_reached_flash_sec`	경유점 도달 LED flash 시간
`wp_file`	waypoint YAML 파일 경로

이 변경 덕분에 launch 파일이나 ros2 run --ros-args -p 명령으로 설정을 바꿀 수 있습니다.

예:

ros2 run your_package robot_state_led_waypoint_follower \
  --ros-args \
  -p wp_file:=/home/robot/waypoints.yaml \
  -p led_topic:=/rgb_led/cmd \
  -p audio_topic:=/audio/command

8) YAML 기반 waypoint 로딩 기능 추가

기존 소스는 waypoint가 코드 내부에 고정되어 있습니다.

self.waypoints = [
    self.create_pose(-0.543, 7.726, 0.25),
    self.create_pose(1.014, 6.529, -1.873),
    self.create_pose(0.997, 3.637, 1.792),
    self.create_pose(1.014, 6.529, -1.873),
]

새 소스는 YAML 파일에서 waypoint를 읽습니다.

self.yaml_data = self.load_waypoint_yaml(self.wp_file)
self.frame_id = self.yaml_data.get("frame_id", "map")

YAML 파일을 읽는 함수도 새로 추가되었습니다.

def load_waypoint_yaml(self, yaml_path: str) -> dict:
    path = Path(yaml_path).expanduser()

    if not path.exists():
        raise FileNotFoundError(f"YAML file not found: {path}")

    with open(path, "r", encoding="utf-8") as file:
        data = yaml.safe_load(file)

    if data is None:
        raise ValueError("YAML file is empty.")

    if "waypoints" not in data:
        raise ValueError("YAML file must contain 'waypoints' field.")

    if not isinstance(data["waypoints"], list):
        raise ValueError("'waypoints' must be a list.")

    return data

이 함수는 파일 존재 여부, YAML이 비어 있는지 여부, waypoints 필드가 있는지 여부, waypoints가 리스트인지 여부를 검사합니다.

예상 YAML 구조는 다음과 같습니다.

frame_id: map

waypoints:
  - name: wp1
    x: -0.543
    y: 7.726
    yaw: 0.25

  - name: wp2
    x: 1.014
    y: 6.529
    yaw: -1.873

9) waypoint 생성 방식 변경

기존 소스는 __init__()에서 바로 self.waypoints 리스트를 만듭니다.

self.waypoints = [
    self.create_pose(-0.543, 7.726, 0.25),
    ...
]

그리고 run()에서 그대로 사용합니다.

self.navigator.followWaypoints(self.waypoints)

새 소스는 run() 안에서 YAML 데이터를 읽어 goal_poses 리스트를 동적으로 생성합니다.

goal_poses = []

for wp in self.yaml_data["waypoints"]:
    name = wp.get("name", "noname")
    x = wp["x"]
    y = wp["y"]
    yaw = wp.get("yaw", 0.0)

    pose = self.create_pose(
        x=x,
        y=y,
        yaw=yaw,
    )

    goal_poses.append(pose)
    self.get_logger().info(f"Loaded waypoint: {name}, x={x}, y={y}, yaw={yaw}")

이 방식은 waypoint 개수가 몇 개든 YAML 파일만 맞으면 자동으로 처리할 수 있습니다.

기존 방식은 waypoint 개수를 늘리려면 코드를 직접 수정해야 했습니다. 새 방식은 파일만 수정하면 됩니다.

10) Nav2 단계별 LED 상태 표시 추가

새 소스는 Nav2 주행 흐름에 맞춰 LED 상태를 계속 바꿉니다.

Nav2 활성화 대기

self.set_robot_state(RobotState.NAV2_WAITING)
self.speak('내비게이션 시스템을 준비합니다')

Nav2가 준비될 때까지 주황색 LED가 켜집니다.

Nav2 준비 완료

self.set_robot_state(RobotState.READY)
self.speak_with_effect('경유점 주행을 시작합니다', 'start')

Nav2가 active 상태가 되면 파란색 LED와 시작 음성이 출력됩니다.

미션 시작

self.set_robot_state(RobotState.MISSION_START)
time.sleep(0.5)

미션 시작 단계에서는 청록색 LED가 잠시 표시됩니다.

이동 중

self.navigator.followWaypoints(goal_poses)
self.set_robot_state(RobotState.MOVING)

waypoint 주행이 시작되면 초록색 LED로 이동 중임을 표시합니다.

성공, 취소, 실패

주행 결과에 따라 상태 LED와 음성 안내가 다르게 출력됩니다.

self.set_robot_state(RobotState.SUCCEEDED)
self.speak_with_effect('모든 경유점 주행을 완료했습니다', 'goal')

self.set_robot_state(RobotState.CANCELED)
self.speak_with_effect('경유점 주행이 취소되었습니다', 'warning')

self.set_robot_state(RobotState.FAILED)
self.speak_with_effect('경유점 주행에 실패했습니다', 'error')

기존 소스도 결과별 음성 안내는 있었지만, LED 상태 표시는 새 소스에서 추가된 부분입니다.

11) 경유점 feedback 처리 방식 변경

기존 소스는 waypoint feedback이 바뀔 때 음성 안내를 출력합니다.

if current_index != last_feedback_index:
    last_feedback_index = current_index

    msg = f'{current_index + 1}번 경유점으로 이동 중입니다'
    self.get_logger().info(msg)
    self.speak(msg)

새 소스는 여기에 경유점 도달 LED flash 기능을 추가했습니다.

if current_index != last_feedback_index:
    if last_feedback_index != -1:
        self.flash_state(
            RobotState.WAYPOINT_REACHED,
            return_state=RobotState.MOVING,
            duration_sec=self.waypoint_reached_flash_sec,
        )

    last_feedback_index = current_index

    msg = f'{current_index + 1}번 경유점으로 이동 중입니다'
    self.get_logger().info(msg)
    self.speak(msg)

차이는 last_feedback_index != -1 조건입니다.

이 조건은 첫 번째 waypoint로 출발할 때는 도달 flash를 하지 않도록 하기 위한 처리입니다. 처음 current_waypoint가 0으로 잡히는 것은 “1번 경유점으로 이동 시작”이지, “경유점 도달”이 아니기 때문입니다.

따라서 새 소스의 동작은 다음처럼 해석할 수 있습니다.

1번 경유점으로 이동 시작 → 음성 안내
2번 경유점으로 index 변경 → 1번 경유점 도달로 보고 보라색 flash
3번 경유점으로 index 변경 → 2번 경유점 도달로 보고 보라색 flash

12) feedback 주기 파라미터화

기존 소스는 feedback loop 마지막에 time.sleep(0.5)를 직접 사용합니다.

time.sleep(0.5)

새 소스는 이 값을 파라미터로 변경했습니다.

self.declare_parameter('feedback_period_sec', 0.5)
self.feedback_period_sec = float(self.get_parameter('feedback_period_sec').value)

그리고 loop에서는 다음처럼 사용합니다.

time.sleep(self.feedback_period_sec)

이제 feedback 확인 주기를 코드 수정 없이 바꿀 수 있습니다.

예를 들어 더 빠른 상태 반응이 필요하면:

-p feedback_period_sec:=0.2

음성 안내나 로그가 너무 자주 나오는 것이 싫으면:

-p feedback_period_sec:=1.0

처럼 설정할 수 있습니다.

13) waypoint 도달 flash 시간 파라미터화

새 소스에는 waypoint_reached_flash_sec 파라미터가 추가되었습니다.

self.declare_parameter('waypoint_reached_flash_sec', 0.3)

이 값은 경유점 도달 시 보라색 LED를 얼마나 오래 표시할지 결정합니다.

duration_sec=self.waypoint_reached_flash_sec

즉, 기본값은 0.3초입니다.

현장에서 LED flash가 너무 짧으면 다음처럼 늘릴 수 있습니다.

-p waypoint_reached_flash_sec:=0.7

14) 종료 시 LED 끄기 기능 추가

새 소스에는 destroy_node()가 재정의되어 있습니다.

def destroy_node(self):
    try:
        self.set_robot_state(RobotState.IDLE)
    except Exception:
        pass

    super().destroy_node()

이 기능은 노드가 종료될 때 LED를 꺼주는 역할을 합니다. IDLE 상태는 RGB 값이 (0, 0, 0)으로 설정되어 있으므로 LED OFF 상태입니다.

기존 소스는 종료 시 노드만 destroy하고 ROS를 shutdown합니다.

finally:
    node.destroy_node()
    rclpy.shutdown()

새 소스도 최종적으로는 destroy_node()를 호출하지만, 그 안에서 LED를 OFF하는 처리가 추가되었습니다.

실제 로봇에서는 프로그램이 종료됐는데 LED가 계속 켜져 있으면 상태를 오해할 수 있습니다. 따라서 종료 시 LED를 끄는 처리는 좋은 추가 기능입니다.

15) 긴급 정지 상태 표시 추가

기존 소스에서도 KeyboardInterrupt는 처리합니다.

except KeyboardInterrupt:
    node.get_logger().warn("Keyboard interrupt received")
    node.speak_with_effect('사용자에 의해 주행이 중지되었습니다', 'warning')

새 소스는 여기에 긴급정지 상태 LED 표시가 추가되었습니다.

except KeyboardInterrupt:
    node.get_logger().warn('Keyboard interrupt received')
    node.set_robot_state(RobotState.EMERGENCY_STOP)
    node.speak_with_effect('사용자에 의해 주행이 중지되었습니다', 'warning')
    time.sleep(0.5)

즉, 사용자가 Ctrl+C로 중단하면 빨간색 LED와 음성 안내가 함께 출력됩니다.

단, 실전 운용 기준으로는 아래 코드도 같이 추가하는 것이 더 안전합니다.

node.navigator.cancelTask()

현재 새 소스는 긴급정지 상태 표시는 하지만, Nav2 task를 명시적으로 cancel하지는 않습니다. 실제 로봇에서는 사용자 중단 시 주행 task도 확실히 취소하는 것이 맞습니다.

5. setup.py 등록 예시

ROS 2 Python 패키지에서 ros2 run으로 실행하려면 setup.py의 entry_points에 등록하는 것이 좋습니다.

예시는 다음과 같습니다.

entry_points={
    'console_scripts': [
            'amcl_waypoint_recorder = tb3_waypoint_nav.amcl_waypoint_recorder:main',
            'waypoint_follower_yaml = tb3_waypoint_nav.waypoint_follower_yaml:main',
            'simple_audio_command_publisher = tb3_waypoint_nav.simple_audio_command_publisher:main',
            'simple_waypoint_follower = tb3_waypoint_nav.simple_waypoint_follower:main',
            'robot_state_led_waypoint_follower = tb3_waypoint_nav.robot_state_led_waypoint_follower:main',
    ],
},

수정 후 빌드합니다.

colcon build --packages-select tb3_waypoint_nav
source install/setup.bash

6. 의존 패키지

이 예제에서 사용하는 주요 의존성은 다음과 같습니다.

rclpy
geometry_msgs
nav2_simple_commander
robot_audio_interfaces
rgb_led_interfaces

package.xml에는 상황에 맞게 다음 의존성을 추가합니다.

<depend>rclpy</depend>
<depend>geometry_msgs</depend>
<depend>nav2_simple_commander</depend>
<depend>robot_audio_interfaces</depend>
<depend>rgb_led_interfaces</depend>

<exec_depend>python3-yaml</exec_depend>

rgb_led_interfaces에는 RgbLed.msg가 있어야 합니다.

예상 메시지 구조는 다음과 같습니다.

bool enable
uint8 r
uint8 g
uint8 b

robot_audio_interfaces에는 AudioCommand.msg가 있어야 합니다.

기존 오디오 시스템을 사용하지 않는다면 오디오 관련 코드를 제거하고 LED 상태 표시만 사용해도 됩니다.

7. LED 출력 + Audio 출력 노드 실행

TurtleBot3의 turtlebot3_ws/src/turtlebot3/turtlebot3_bringup/launch에 audio_led_nodes.launch.py이름으로 아래의 파일을 생성합니다.

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument


def generate_launch_description():

    port = LaunchConfiguration('port', default='/dev/tb3_sensor')

    return LaunchDescription([
        DeclareLaunchArgument(
            'port',
            default_value=port,
            description='Connected USB port with Arduino Uno Board'),

        Node(
            package='rgb_led_serial',
            executable='serial_led_node',
            name='serial_led_node',
            parameters=[
                {'port': port},
            ],
            output='screen',
        ),

        Node(
            package='robot_audio_output',
            executable='audio_output_node',
            name='audio_output_node',           
            output='screen',
       )
    ])

.bashrc에 rgb_ed_ws의 활성화 명령어도 추가합니다.

echo 'source ~/rgb_led_ws/install/setup.bash' >> ~/.bashrc
source ~/.bashrc

9. 노드 실행

1) 로봇 터미널 1 : turtlebot3_bringup 실행

ros2 launch turtlebot3_bringup robot.launch.py

2) 로봇 터미널 2 : audio_led_nodes 실행

ros2 launch turtlebot3_bringup audio_led_nodes.launch.py

3) Remote PC 터미널 3: Nav2 실행

경유점 주행 노드는 Nav2가 실행된 상태에서 동작합니다. 아래의 명령 중 개인환경에 적합하게 수정한 후 실행하시기 바랍니다.

ros2 launch turtlebot3_navigation2 navigation2.launch.py map:=/home/sjyong/turtlebot3_ws/src/turtlebot3/turtlebot3_navigation2/map/map.yaml

4) Remote PC 터미널 4: 로봇 상태 LED 경유점 주행 노드 실행

Nav2가 준비된 뒤 이번에 만든 노드를 실행합니다.

ros2 run tb3_waypoint_nav robot_state_led_waypoint_follower --ros-args -p wp_file:=/home/sjyong/turtlebot3_ws/src/tb3_waypoint_nav/waypoints/recorded_waypoints.yaml

파라미터를 지정해서 실행할 수도 있습니다.

ros2 run robot_status_led robot_state_led_waypoint_follower \
  --ros-args \
  -p led_topic:=/rgb_led/cmd \
  -p audio_topic:=/audio/command \
  -p feedback_period_sec:=0.5 \
  -p waypoint_reached_flash_sec:=0.3

실행하면 로봇 상태에 따라 LED 색상이 바뀝니다.

BOOTING        → 흰색
NAV2_WAITING   → 주황색
READY          → 파란색
MISSION_START  → 청록색
MOVING         → 초록색
SUCCEEDED      → 초록색
FAILED         → 빨간색
CANCELED       → 노란색

5) Remote PC 터미널 5: RGB LED 토픽 확인

LED 메시지가 정상적으로 발행되는지 확인합니다.

ros2 topic echo /rgb_led/cmd

예상 출력은 다음과 같습니다.

enable: true
r: 0
g: 255
b: 0

이 값은 초록색을 의미합니다.

R = 0
G = 255
B = 0

즉, 로봇이 주행 중이거나 미션 성공 상태일 가능성이 높습니다.

6) 수동으로 LED 테스트하기

주행 노드를 실행하기 전에 LED만 먼저 테스트할 수도 있습니다.

ros2 topic pub /rgb_led/cmd rgb_led_interfaces/msg/RgbLed \
"{enable: true, r: 255, g: 0, b: 0}"

위 명령은 LED를 빨간색으로 켭니다.

초록색 테스트:

ros2 topic pub /rgb_led/cmd rgb_led_interfaces/msg/RgbLed \
"{enable: true, r: 0, g: 255, b: 0}"

파란색 테스트:

ros2 topic pub /rgb_led/cmd rgb_led_interfaces/msg/RgbLed \
"{enable: true, r: 0, g: 0, b: 255}"

LED 끄기:

ros2 topic pub /rgb_led/cmd rgb_led_interfaces/msg/RgbLed \
"{enable: false, r: 0, g: 0, b: 0}"

이 테스트가 정상적으로 동작하면 RGB LED 시리얼 노드는 제대로 동작하는 것입니다.
배터리 전압이 낮아지면 LED를 주황색 또는 빨간색으로 표시할 수 있습니다.

LOW_BATTERY = 'low_battery'

RobotState.LOW_BATTERY: (255, 80, 0)

2) 장애물 감지 상태

장애물이 감지되면 노란색 점멸로 표시할 수 있습니다.

OBSTACLE_DETECTED = 'obstacle_detected'

RobotState.OBSTACLE_DETECTED: (255, 255, 0)

3) 수동 조작 모드

자율주행이 아니라 조이스틱이나 RC로 조작 중이라면 별도 색상을 줄 수 있습니다.

MANUAL_CONTROL = 'manual_control'

RobotState.MANUAL_CONTROL: (0, 120, 255)

4) 도킹 상태

충전 도킹 중이라면 파란색 점멸, 충전 완료라면 초록색으로 표시할 수 있습니다.

DOCKING = 'docking'
CHARGING = 'charging'
CHARGE_COMPLETE = 'charge_complete'

10. device rules 파일

99-turtlebot3-cdc.rules 파일을 수정합니다. 아래와 같이 첫번째 줄을 주석처리합니다.


#ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", ENV{ID_MM_DEVICE_IGNORE}="1", MODE:="0666"
ATTRS{idVendor}=="0483", ATTRS{idProduct}=="df11", MODE:="0666"
ATTRS{idVendor}=="fff1", ATTRS{idProduct}=="ff48", ENV{ID_MM_DEVICE_IGNORE}="1", MODE:="0666"
SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", ATTRS{serial}=="0001", ENV{ID_MM_DEVICE_IGNORE}="1", MODE:="0666", SYMLINK+="tb3_lidar"

파일 이름 : 99-arduino-opencr.rules

ACTION=="add", SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", MODE:="0666", SYMLINK+="tb3_opencr"
ACTION=="add", SUBSYSTEM=="tty", ATTRS{idVendor}=="2341", ATTRS{idProduct}=="0043", MODE:="0666", SYMLINK+="tb3_sensor"

위의 파일을 /etc/udev/rules.d/에 복사합니다.

sudo cp 99-arduino-opencr.rules /etc/udev/rules.d/
sudo udevadm control --reload-rules
sudo udevadm trigger

cd /dev
ls -al

turtlebot3 bringup 실행

 ros2 launch turtlebot3_bringup robot.launch.py usb_port:=/dev/tb3_opencr

audio_led_nodes.launch.py 실행

ros2 launch turtlebot3_bringup audio_led_nodes.launch.py port:=/dev/tb3_sensor