Step 1: 로봇 설정 및 기본 패키지 생성

먼저 프로젝트를 위한 워크스페이스와 패키지를 생성해요:

mkdir -p ~/autonomous_robot_ws/src
cd ~/autonomous_robot_ws/src
ros2 pkg create --build-type ament_python autonomous_robot --dependencies rclpy sensor_msgs geometry_msgs nav_msgs cv_bridge

Step 2: 로봇 제어 노드 구현

로봇의 전체 동작을 관리하는 메인 노드를 구현해요:

import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from nav2_msgs.action import NavigateToPose
from sensor_msgs.msg import LaserScan, Image
from geometry_msgs.msg import Twist, PoseStamped
from cv_bridge import CvBridge
import cv2
import numpy as np
import threading
import time

class AutonomousRobot(Node):
    def __init__(self):
        super().__init__('autonomous_robot')
        
        # 라이다 데이터 구독
        self.laser_subscription = self.create_subscription(
            LaserScan,
            'scan',
            self.laser_callback,
            10)
            
        # 카메라 이미지 구독
        self.image_subscription = self.create_subscription(
            Image,
            'camera/image_raw',
            self.image_callback,
            10)
            
        # 로봇 제어를 위한 발행자
        self.cmd_vel_publisher = self.create_publisher(
            Twist,
            'cmd_vel',
            10)
            
        # 내비게이션 액션 클라이언트
        self.nav_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
        
        # OpenCV 브릿지
        self.bridge = CvBridge()
        
        # 로봇 상태
        self.robot_state = 'IDLE'  # 'IDLE', 'EXPLORING', 'NAVIGATING', 'OBSTACLE_AVOIDING'
        self.obstacle_detected = False
        self.current_image = None
        
        # 내비게이션 목표점 리스트
        self.waypoints = [
            (1.0, 0.0, 0.0),  # (x, y, theta)
            (1.0, 1.0, 1.57),
            (0.0, 1.0, 3.14),
            (0.0, 0.0, 0.0)
        ]
        self.current_waypoint_index = 0
        
        # 타이머 설정
        self.state_timer = self.create_timer(0.5, self.state_machine)
        
        self.get_logger().info('자율주행 로봇 초기화 완료!')
        
    def laser_callback(self, msg):
        """라이다 데이터 처리"""
        # 전방 180도 내에 장애물이 있는지 확인
        front_ranges = msg.ranges[0:30] + msg.ranges[330:360]
        min_distance = min([r for r in front_ranges if not np.isinf(r)], default=10.0)
        
        # 장애물 감지 임계값
        if min_distance < 0.5:
            self.obstacle_detected = True
            if self.robot_state == 'NAVIGATING':
                self.robot_state = 'OBSTACLE_AVOIDING'
        else:
            self.obstacle_detected = False
            
    def image_callback(self, msg):
        """카메라 이미지 처리"""
        try:
            self.current_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
            
            # 여기에 객체 인식 코드를 추가할 수 있어요
            # 간단한 예로, 빨간색 물체 감지
            hsv = cv2.cvtColor(self.current_image, cv2.COLOR_BGR2HSV)
            lower_red = np.array([0, 100, 100])
            upper_red = np.array([10, 255, 255])
            mask = cv2.inRange(hsv, lower_red, upper_red)
            
            # 빨간색 물체가 감지되면 로그 출력
            if np.sum(mask) > 10000:
                self.get_logger().info('빨간색 물체 감지됨!')
                
        except Exception as e:
            self.get_logger().error(f'이미지 처리 오류: {str(e)}')
            
    def navigate_to_waypoint(self, waypoint_index):
        """지정된 웨이포인트로 이동"""
        if not self.nav_client.wait_for_server(timeout_sec=5.0):
            self.get_logger().error('내비게이션 서버에 연결할 수 없습니다!')
            return False
            
        x, y, theta = self.waypoints[waypoint_index]
        
        # 목표 포즈 생성
        goal_msg = NavigateToPose.Goal()
        goal_msg.pose.header.frame_id = 'map'
        goal_msg.pose.header.stamp = self.get_clock().now().to_msg()
        
        # 위치 설정
        goal_msg.pose.pose.position.x = x
        goal_msg.pose.pose.position.y = y
        goal_msg.pose.pose.position.z = 0.0
        
        # 방향 설정 (쿼터니언)
        from math import sin, cos
        cy = cos(theta * 0.5)
        sy = sin(theta * 0.5)
        goal_msg.pose.pose.orientation.x = 0.0
        goal_msg.pose.pose.orientation.y = 0.0
        goal_msg.pose.pose.orientation.z = sy
        goal_msg.pose.pose.orientation.w = cy
        
        self.get_logger().info(f'웨이포인트 {waypoint_index}로 이동 중: ({x}, {y}, {theta})')
        
        # 목표 전송
        send_goal_future = self.nav_client.send_goal_async(
            goal_msg, 
            feedback_callback=self.navigation_feedback_callback)
        send_goal_future.add_done_callback(self.navigation_response_callback)
        
        return True
        
    def navigation_response_callback(self, future):
        """내비게이션 요청 응답 처리"""
        goal_handle = future.result()
        
        if not goal_handle.accepted:
            self.get_logger().error('내비게이션 목표가 거부되었습니다')
            self.robot_state = 'IDLE'
            return
            
        self.get_logger().info('내비게이션 목표가 수락되었습니다')
        
        # 결과 요청
        result_future = goal_handle.get_result_async()
        result_future.add_done_callback(self.navigation_result_callback)
        
    def navigation_feedback_callback(self, feedback_msg):
        """내비게이션 피드백 처리"""
        feedback = feedback_msg.feedback
        # 필요한 경우 피드백 처리 코드 추가
        
    def navigation_result_callback(self, future):
        """내비게이션 결과 처리"""
        status = future.result().status
        
        if status == 4:  # 성공
            self.get_logger().info('웨이포인트에 도착했습니다!')
            self.current_waypoint_index = (self.current_waypoint_index + 1) % len(self.waypoints)
            self.robot_state = 'IDLE'
        else:
            self.get_logger().info(f'내비게이션 완료, 상태: {status}')
            self.robot_state = 'IDLE'
            
    def avoid_obstacle(self):
        """장애물 회피 동작"""
        twist = Twist()
        
        # 간단한 회피 알고리즘: 제자리에서 회전
        twist.linear.x = 0.0
        twist.angular.z = 0.5  # 좌회전
        
        self.cmd_vel_publisher.publish(twist)
        self.get_logger().info('장애물 회피 중...')
        
    def explore(self):
        """탐색 동작"""
        twist = Twist()
        
        # 간단한 탐색 알고리즘: 느리게 전진하며 주변 탐색
        twist.linear.x = 0.1
        twist.angular.z = 0.1  # 약간 좌회전하며 탐색
        
        self.cmd_vel_publisher.publish(twist)
        
    def stop(self):
        """로봇 정지"""
        twist = Twist()
        twist.linear.x = 0.0
        twist.angular.z = 0.0
        self.cmd_vel_publisher.publish(twist)
        
    def state_machine(self):
        """로봇 상태 머신"""
        if self.robot_state == 'IDLE':
            self.stop()
            # 잠시 대기 후 내비게이션 시작
            self.robot_state = 'NAVIGATING'
            self.navigate_to_waypoint(self.current_waypoint_index)
            
        elif self.robot_state == 'EXPLORING':
            self.explore()
            
        elif self.robot_state == 'NAVIGATING':
            # 내비게이션은 액션 서버에서 처리 중
            pass
            
        elif self.robot_state == 'OBSTACLE_AVOIDING':
            self.avoid_obstacle()
            
            # 장애물이 없어지면 다시 내비게이션으로 전환
            if not self.obstacle_detected:
                self.robot_state = 'NAVIGATING'
                self.navigate_to_waypoint(self.current_waypoint_index)

def main(args=None):
    rclpy.init(args=args)
    robot = AutonomousRobot()
    
    try:
        rclpy.spin(robot)
    except KeyboardInterrupt:
        pass
    finally:
        # 정리
        robot.stop()  # 로봇 정지
        robot.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

Step 3: 음성 인식 노드 구현

음성 명령을 인식하는 노드를 추가해요:

import rclpy
from rclpy.node import Node
from std_msgs.msg import String
import speech_recognition as sr
import threading

class VoiceCommandNode(Node):
    def __init__(self):
        super().__init__('voice_command_node')
        
        # 음성 명령 발행자
        self.command_publisher = self.create_publisher(
            String,
            'voice_commands',
            10)
            
        # 음성 인식기 초기화
        self.recognizer = sr.Recognizer()
        self.microphone = sr.Microphone()
        
        # 배경 소음 조정
        with self.microphone as source:
            self.get_logger().info('배경 소음 조정 중...')
            self.recognizer.adjust_for_ambient_noise(source)
            self.get_logger().info('음성 인식 준비 완료!')
            
        # 별도 스레드에서 음성 인식 실행
        self.stop_listening = False
        self.listen_thread = threading.Thread(target=self.listen_for_commands)
        self.listen_thread.start()
        
    def listen_for_commands(self):
        """음성 명령 인식 루프"""
        while not self.stop_listening:
            try:
                with self.microphone as source:
                    self.get_logger().info('명령을 말해주세요...')
                    audio = self.recognizer.listen(source, timeout=5.0, phrase_time_limit=5.0)
                    
                try:
                    # 구글 음성 인식 API 사용
                    command = self.recognizer.recognize_google(audio, language='ko-KR')
                    self.get_logger().info(f'인식된 명령: {command}')
                    
                    # 명령 발행
                    msg = String()
                    msg.data = command
                    self.command_publisher.publish(msg)
                    
                    # 종료 명령 처리
                    if '종료' in command or '정지' in command:
                        self.get_logger().info('종료 명령 감지. 음성 인식을 종료합니다.')
                        self.stop_listening = True
                        break
                        
                except sr.UnknownValueError:
                    self.get_logger().info('음성을 인식할 수 없습니다')
                except sr.RequestError as e:
                    self.get_logger().error(f'음성 인식 서비스 오류: {e}')
                    
            except Exception as e:
                self.get_logger().error(f'음성 인식 오류: {str(e)}')
                time.sleep(1)
                
        self.get_logger().info('음성 인식 스레드 종료')
        
    def destroy_node(self):
        self.stop_listening = True
        self.listen_thread.join(timeout=5.0)
        super().destroy_node()

def main(args=None):
    rclpy.init(args=args)
    voice_node = VoiceCommandNode()
    
    try:
        rclpy.spin(voice_node)
    except KeyboardInterrupt:
        pass
    finally:
        voice_node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

Step 4: 패키지 설정 및 빌드

setup.py 파일을 수정하여 노드들을 실행 가능하게 만들어요:

from setuptools import setup

package_name = 'autonomous_robot'

setup(
    name=package_name,
    version='0.0.1',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Your Name',
    maintainer_email='your.email@example.com',
    description='Autonomous robot project',
    license='Apache License 2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'robot_controller = autonomous_robot.robot_controller:main',
            'voice_command = autonomous_robot.voice_command:main',
        ],
    },
)

패키지를 빌드하고 설치해요:

cd ~/autonomous_robot_ws
colcon build --packages-select autonomous_robot
source install/setup.bash

Step 5: 로봇 실행

먼저 필요한 기본 노드들을 실행해요:

# 터미널 1: SLAM 실행
ros2 launch slam_toolbox online_async_launch.py

# 터미널 2: 내비게이션 실행
ros2 launch nav2_bringup navigation_launch.py

# 터미널 3: 로봇 기본 드라이버 실행 (TurtleBot3 예시)
ros2 launch turtlebot3_bringup turtlebot3_robot.launch.py

이제 우리가 만든 노드들을 실행해요:

# 터미널 4: 로봇 제어 노드 실행
ros2 run autonomous_robot robot_controller

# 터미널 5: 음성 명령 노드 실행
ros2 run autonomous_robot voice_command