PyQt로 Waypoint/Trajectory 선택 주행 GUI 만들기

by

1. 개요

이번 실습에서는 Ubuntu 22.04 Server가 설치된 TurtleBot3 Burger에서 ROS 2 Humble과 Nav2를 사용해 waypoint와 trajectory를 선택 주행하는 PyQt GUI 프로그램을 만들겠습니다.

YAML 파일에는 두 가지 정보가 들어 있습니다.

  1. waypoints: 실제 지도 좌표
  2. trajectories: waypoint 이름을 순서대로 나열한 주행 경로

Nav2 Humble에서 단일 목적지 이동은 NavigateToPose 액션을 사용하고, 여러 waypoint 순차 주행은 FollowWaypoints 액션을 사용합니다. FollowWaypoints의 goal은 geometry_msgs/PoseStamped[] 배열을 받기 때문에 YAML에 정의된 waypoint 이름들을 실제 PoseStamped 배열로 변환해서 보내면 됩니다.

2. 최종 동작 구조

이 예제의 동작 구조는 다음과 같습니다.

  1. PyQt GUI 실행
  2. YAML 파일 읽기
  3. waypoint 목록을 waypoint 콤보박스에 등록
  4. trajectory 목록을 trajectory 콤보박스에 등록
  5. waypoint 선택 후 주행 버튼 클릭
  6. Nav2 /navigate_to_pose 액션으로 단일 목적지 전송
  7. trajectory 선택 후 주행 버튼 클릭
  8. trajectory에 포함된 waypoint 이름을 실제 pose 배열로 변환
  9. Nav2 /follow_waypoints 액션으로 순차 주행 명령 전송
  10. 중지 버튼 클릭 시 현재 Nav2 goal 취소
  11. YAML 파일 형식

기존에 사용 중인 waypoints.yaml 파일은 아래와 같은 구조입니다.

waypoints:
  - name: point1
    frame_id: map
    pose:
      position:
        x: 32.5246347706
        y: -9.05557767837
        z: 0.0
      angle:
        yaw: 89.9111495477

trajectories:
  - name: Traj1
    waypoints:
    - point2
    - point3
    - point5
    - point6

여기서 중요한 점은 trajectories 안에는 좌표가 직접 들어 있지 않고 waypoint 이름만 들어 있다는 것입니다.

예를 들어 Traj1이 다음과 같다면,

trajectories:
  - name: Traj1
    waypoints:
    - point2
    - point3
    - point5

프로그램은 내부적으로 point2, point3, point5를 찾아서 각각 PoseStamped로 변환한 뒤 Nav2 FollowWaypoints 액션으로 보냅니다.

3. 패키지 생성

ROS 2 워크스페이스에서 패키지를 만듭니다.

cd ~/pyqt_ws/src

ros2 pkg create tb3_nav2_pyqt_gui \
  --build-type ament_python \
  --dependencies rclpy geometry_msgs nav2_msgs ament_index_python

패키지 구조는 다음과 같이 맞춥니다.

tb3_nav2_pyqt_gui/
├── config/
│   └── waypoints.yaml
├── launch/
│   └── nav2_waypoint_gui.launch.py
├── package.xml
├── setup.cfg
├── setup.py
├── resource/
│   └── tb3_nav2_pyqt_gui
└── tb3_nav2_pyqt_gui/
    ├── __init__.py
    └── nav2_waypoint_gui.py

waypoints.yaml 파일을 아래 위치에 복사합니다.

mkdir -p ~/pyqt_ws/src/tb3_nav2_pyqt_gui/config
cd /Downloads
cp ~/Downloads/waypoints.yaml tb3_nav2_pyqt_gui/config/

4. 필요한 패키지 설치

PyQt와 YAML 파서를 설치합니다.

sudo apt update
sudo apt install -y python3-pyqt5 python3-yaml

Nav2 관련 패키지가 없다면 설치합니다.

sudo apt install -y ros-humble-navigation2 ros-humble-nav2-bringup

5. PyQt + Nav2 GUI 전체 소스

tb3_nav2_pyqt_gui/nav2_waypoint_gui.py 파일을 아래처럼 작성합니다.

cd ~/pyqt_ws/src/tb3_nav2_pyqt_gui/tb3_nav2_pyqt_gui/
touch nav2_waypoint_gui.py
#!/usr/bin/env python3

import sys
import math
import argparse
import yaml

import rclpy
from rclpy.action import ActionClient
from rclpy.utilities import remove_ros_args

from geometry_msgs.msg import PoseStamped
from nav2_msgs.action import NavigateToPose
from nav2_msgs.action import FollowWaypoints

from PyQt5.QtCore import QTimer
from PyQt5.QtWidgets import (
    QApplication,
    QMainWindow,
    QWidget,
    QLabel,
    QComboBox,
    QPushButton,
    QTextEdit,
    QVBoxLayout,
    QHBoxLayout,
    QGroupBox,
)


class Nav2WaypointGui(QMainWindow):
    def __init__(self, yaml_file):
        super().__init__()

        self.yaml_file = yaml_file

        self.waypoints = {}
        self.trajectories = {}

        self.node = rclpy.create_node('simple_nav2_waypoint_gui')

        self.navigate_client = ActionClient(
            self.node,
            NavigateToPose,
            'navigate_to_pose'
        )

        self.follow_client = ActionClient(
            self.node,
            FollowWaypoints,
            'follow_waypoints'
        )

        self.setWindowTitle('TurtleBot3 Nav2 Waypoint GUI')
        self.resize(600, 420)

        self.make_gui()
        self.load_yaml()

        self.timer = QTimer()
        self.timer.timeout.connect(self.ros_spin_once)
        self.timer.start(50)

    def make_gui(self):
        main_widget = QWidget()
        main_layout = QVBoxLayout()

        yaml_group = QGroupBox('YAML 파일')
        yaml_layout = QVBoxLayout()
        yaml_layout.addWidget(QLabel(self.yaml_file))
        yaml_group.setLayout(yaml_layout)

        waypoint_group = QGroupBox('Waypoint 단일 주행')
        waypoint_layout = QVBoxLayout()

        self.waypoint_combo = QComboBox()
        self.waypoint_button = QPushButton('선택한 Waypoint로 이동')
        self.waypoint_button.clicked.connect(self.go_to_waypoint)

        waypoint_layout.addWidget(QLabel('Waypoint 선택'))
        waypoint_layout.addWidget(self.waypoint_combo)
        waypoint_layout.addWidget(self.waypoint_button)
        waypoint_group.setLayout(waypoint_layout)

        trajectory_group = QGroupBox('Trajectory 순차 주행')
        trajectory_layout = QVBoxLayout()

        self.trajectory_combo = QComboBox()
        self.trajectory_label = QLabel('')
        self.trajectory_button = QPushButton('선택한 Trajectory 주행')
        self.trajectory_button.clicked.connect(self.go_to_trajectory)
        self.trajectory_combo.currentIndexChanged.connect(
            self.show_trajectory_info
        )

        trajectory_layout.addWidget(QLabel('Trajectory 선택'))
        trajectory_layout.addWidget(self.trajectory_combo)
        trajectory_layout.addWidget(self.trajectory_label)
        trajectory_layout.addWidget(self.trajectory_button)
        trajectory_group.setLayout(trajectory_layout)

        log_group = QGroupBox('로그')
        log_layout = QVBoxLayout()

        self.log_box = QTextEdit()
        self.log_box.setReadOnly(True)

        log_layout.addWidget(self.log_box)
        log_group.setLayout(log_layout)

        main_layout.addWidget(yaml_group)
        main_layout.addWidget(waypoint_group)
        main_layout.addWidget(trajectory_group)
        main_layout.addWidget(log_group)

        main_widget.setLayout(main_layout)
        self.setCentralWidget(main_widget)

    def load_yaml(self):
        with open(self.yaml_file, 'r', encoding='utf-8') as f:
            data = yaml.safe_load(f)

        waypoint_list = data['waypoints']
        trajectory_list = data['trajectories']

        for wp in waypoint_list:
            name = wp['name']
            self.waypoints[name] = wp
            self.waypoint_combo.addItem(name)

        for traj in trajectory_list:
            name = traj['name']
            wp_names = traj['waypoints']
            self.trajectories[name] = wp_names
            self.trajectory_combo.addItem(name)

        self.show_trajectory_info()

        self.log('YAML 로드 완료')
        self.log(f'Waypoint 개수: {len(self.waypoints)}')
        self.log(f'Trajectory 개수: {len(self.trajectories)}')

    def show_trajectory_info(self):
        traj_name = self.trajectory_combo.currentText()

        if traj_name in self.trajectories:
            wp_names = self.trajectories[traj_name]
            text = ' -> '.join(wp_names)
            self.trajectory_label.setText(text)

    def make_pose(self, waypoint_name):
        wp = self.waypoints[waypoint_name]

        frame_id = wp.get('frame_id', 'map')

        position = wp['pose']['position']
        angle = wp['pose']['angle']

        x = float(position['x'])
        y = float(position['y'])

        # TurtleBot3 Burger는 2D 주행 로봇이므로 z는 0으로 고정한다.
        z = 0.0

        yaw_deg = float(angle['yaw'])
        yaw_rad = math.radians(yaw_deg)

        qz = math.sin(yaw_rad / 2.0)
        qw = math.cos(yaw_rad / 2.0)

        pose = PoseStamped()
        pose.header.frame_id = frame_id
        pose.header.stamp = self.node.get_clock().now().to_msg()

        pose.pose.position.x = x
        pose.pose.position.y = y
        pose.pose.position.z = z

        pose.pose.orientation.x = 0.0
        pose.pose.orientation.y = 0.0
        pose.pose.orientation.z = qz
        pose.pose.orientation.w = qw

        return pose

    def go_to_waypoint(self):
        waypoint_name = self.waypoint_combo.currentText()

        if waypoint_name == '':
            self.log('선택된 waypoint가 없습니다.')
            return

        if not self.navigate_client.wait_for_server(timeout_sec=1.0):
            self.log('/navigate_to_pose 액션 서버가 준비되지 않았습니다.')
            return

        goal_msg = NavigateToPose.Goal()
        goal_msg.pose = self.make_pose(waypoint_name)
        goal_msg.behavior_tree = ''

        self.log(f'Waypoint 이동 요청: {waypoint_name}')

        future = self.navigate_client.send_goal_async(goal_msg)
        future.add_done_callback(self.waypoint_goal_response)

    def waypoint_goal_response(self, future):
        goal_handle = future.result()

        if not goal_handle.accepted:
            self.log('Waypoint goal이 거부되었습니다.')
            return

        self.log('Waypoint goal이 수락되었습니다.')

        result_future = goal_handle.get_result_async()
        result_future.add_done_callback(self.waypoint_result)

    def waypoint_result(self, future):
        self.log('Waypoint 이동 완료')

    def go_to_trajectory(self):
        traj_name = self.trajectory_combo.currentText()

        if traj_name == '':
            self.log('선택된 trajectory가 없습니다.')
            return

        if traj_name not in self.trajectories:
            self.log('trajectory 정보가 없습니다.')
            return

        if not self.follow_client.wait_for_server(timeout_sec=1.0):
            self.log('/follow_waypoints 액션 서버가 준비되지 않았습니다.')
            return

        waypoint_names = self.trajectories[traj_name]

        poses = []

        for name in waypoint_names:
            if name not in self.waypoints:
                self.log(f'YAML에 없는 waypoint입니다: {name}')
                return

            pose = self.make_pose(name)
            poses.append(pose)

        goal_msg = FollowWaypoints.Goal()
        goal_msg.poses = poses

        self.log(f'Trajectory 주행 요청: {traj_name}')
        self.log(f'포함된 waypoint 개수: {len(poses)}')

        future = self.follow_client.send_goal_async(goal_msg)
        future.add_done_callback(self.trajectory_goal_response)

    def trajectory_goal_response(self, future):
        goal_handle = future.result()

        if not goal_handle.accepted:
            self.log('Trajectory goal이 거부되었습니다.')
            return

        self.log('Trajectory goal이 수락되었습니다.')

        result_future = goal_handle.get_result_async()
        result_future.add_done_callback(self.trajectory_result)

    def trajectory_result(self, future):
        self.log('Trajectory 주행 완료')

    def ros_spin_once(self):
        rclpy.spin_once(self.node, timeout_sec=0.0)

    def log(self, msg):
        self.log_box.append(msg)
        self.node.get_logger().info(msg)

    def closeEvent(self, event):
        self.timer.stop()
        self.node.destroy_node()
        event.accept()


def main():
    rclpy.init(args=sys.argv)

    ros_removed_args = remove_ros_args(sys.argv)

    parser = argparse.ArgumentParser()
    parser.add_argument(
        '--yaml',
        required=True,
        help='waypoint yaml file path'
    )

    args = parser.parse_args(ros_removed_args[1:])

    app = QApplication(ros_removed_args)

    window = Nav2WaypointGui(args.yaml)
    window.show()

    app.exec_()

    rclpy.shutdown()


if __name__ == '__main__':
    main()

6. 코드 설명

이 소스는 Nav2WaypointGui 클래스 하나로 구성되어 있습니다.

class Nav2WaypointGui(QMainWindow):

이 클래스 안에서 다음 작업을 모두 처리합니다.

  1. PyQt 화면 만들기
  2. YAML 파일 읽기
  3. waypoint 콤보박스 등록
  4. trajectory 콤보박스 등록
  5. Nav2 액션 서버에 goal 전송
  6. ROS 2 spin 처리
  7. YAML 읽기 부분
with open(self.yaml_file, 'r', encoding='utf-8') as f:
    data = yaml.safe_load(f)

YAML 파일을 읽은 뒤 waypoints와 trajectories를 분리합니다.

waypoint_list = data['waypoints']
trajectory_list = data['trajectories']

waypoint는 이름으로 쉽게 찾기 위해 dictionary로 저장합니다.

self.waypoints[name] = wp

예를 들어 YAML에 point1, point2, point3이 있으면 내부적으로 다음과 비슷하게 저장됩니다.

self.waypoints['point1']
self.waypoints['point2']
self.waypoints['point3']

trajectory도 이름으로 찾기 쉽게 저장합니다.

self.trajectories[name] = wp_names

예를 들어 YAML에 Traj1이 있으면 다음과 같이 접근할 수 있습니다.

self.trajectories['Traj1']

waypoint 콤보박스 등록

self.waypoint_combo.addItem(name)

YAML에 있는 waypoint 이름을 GUI 콤보박스에 추가합니다.

예를 들어 YAML에 아래 waypoint가 있으면,

- name: point1
- name: point2
- name: point3

GUI에는 다음 항목이 표시됩니다.

point1
point2
point3

trajectory 콤보박스 등록

self.trajectory_combo.addItem(name)

YAML에 있는 trajectory 이름을 GUI 콤보박스에 추가합니다.

예를 들어 YAML에 아래 내용이 있으면,

trajectories:
  - name: Traj1
  - name: Traj2
  - name: Traj3

GUI에는 다음 항목이 표시됩니다.

Traj1
Traj2
Traj3

trajectory 내용 표시

text = ' -> '.join(wp_names)
self.trajectory_label.setText(text)

trajectory를 선택하면 포함된 waypoint 순서를 화면에 보여줍니다.

예를 들어 Traj2가 다음과 같다면,

waypoints:
- point2
- point3
- point4
- point3
- point1

GUI에는 다음처럼 표시됩니다.

point2 -> point3 -> point4 -> point3 -> point1

waypoint를 PoseStamped로 변환하는 부분

Nav2 액션은 단순한 x, y, yaw 값을 직접 받지 않습니다. ROS 2의 PoseStamped 메시지로 변환해서 보내야 합니다.

pose = PoseStamped()
pose.header.frame_id = frame_id
pose.header.stamp = self.node.get_clock().now().to_msg()

좌표값은 YAML에서 읽습니다.

x = float(position['x'])
y = float(position['y'])

yaw를 quaternion으로 변환하는 부분

YAML의 yaw 값은 degree 단위입니다.

yaw_deg = float(angle['yaw'])
yaw_rad = math.radians(yaw_deg)

ROS 2 pose orientation은 quaternion을 사용하므로 yaw 값을 quaternion으로 변환합니다.

qz = math.sin(yaw_rad / 2.0)
qw = math.cos(yaw_rad / 2.0)

2D 로봇에서는 roll, pitch를 사용하지 않으므로 x, y quaternion은 0으로 둡니다.

pose.pose.orientation.x = 0.0
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = qz
pose.pose.orientation.w = qw

단일 waypoint 주행 부분

버튼을 누르면 현재 선택된 waypoint 이름을 가져옵니다.

waypoint_name = self.waypoint_combo.currentText()

그리고 NavigateToPose goal을 만듭니다.

goal_msg = NavigateToPose.Goal()
goal_msg.pose = self.make_pose(waypoint_name)
goal_msg.behavior_tree = ''

이 goal을 /navigate_to_pose 액션 서버로 보냅니다.

future = self.navigate_client.send_goal_async(goal_msg)

이 부분이 RViz에서 Nav2 Goal을 찍는 것과 비슷한 역할입니다.

trajectory 주행 부분

trajectory 버튼을 누르면 현재 선택된 trajectory 이름을 가져옵니다.

traj_name = self.trajectory_combo.currentText()

그 trajectory에 들어 있는 waypoint 이름 목록을 가져옵니다.

waypoint_names = self.trajectories[traj_name]

각 waypoint 이름을 실제 pose로 변환합니다.

poses = []

for name in waypoint_names:
    pose = self.make_pose(name)
    poses.append(pose)

그 다음 FollowWaypoints goal을 만듭니다.

goal_msg = FollowWaypoints.Goal()
goal_msg.poses = poses

마지막으로 /follow_waypoints 액션 서버로 보냅니다.

future = self.follow_client.send_goal_async(goal_msg)

Nav2 Waypoint Follower는 받은 waypoint들을 순서대로 주행하는 기능입니다. Nav2 문서에서도 Waypoint Follower가 정렬된 waypoint들을 받아 순서대로 이동하는 구조라고 설명합니다.

ROS 2 spin 처리

PyQt는 GUI 이벤트 루프를 사용합니다.

app.exec_()

ROS 2는 callback 처리를 위해 spin이 필요합니다.

rclpy.spin_once(self.node, timeout_sec=0.0)

두 개를 동시에 사용하기 위해 QTimer를 사용했습니다.

self.timer = QTimer()
self.timer.timeout.connect(self.ros_spin_once)
self.timer.start(50)

즉, GUI가 실행되는 동안 50ms마다 ROS 2 callback을 한 번씩 처리합니다.

7. package.xml 작성

package.xml 파일을 아래처럼 작성합니다.

<?xml version="1.0"?>
<package format="3">
  <name>tb3_nav2_pyqt_gui</name>
  <version>0.0.1</version>
  <description>TurtleBot3 Nav2 waypoint and trajectory PyQt GUI</description>

  <maintainer email="user@example.com">user</maintainer>
  <license>Apache-2.0</license>

  <buildtool_depend>ament_python</buildtool_depend>

  <exec_depend>rclpy</exec_depend>
  <exec_depend>geometry_msgs</exec_depend>
  <exec_depend>nav2_msgs</exec_depend>
  <exec_depend>ament_index_python</exec_depend>
  <exec_depend>launch</exec_depend>
  <exec_depend>launch_ros</exec_depend>
  <exec_depend>python3-pyqt5</exec_depend>
  <exec_depend>python3-yaml</exec_depend>

  <export>
    <build_type>ament_python</build_type>
  </export>
</package>

8. setup.py 작성

setup.py 파일을 아래처럼 작성합니다.

from setuptools import setup
from glob import glob
import os

package_name = 'tb3_nav2_pyqt_gui'

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']
        ),
        (
            os.path.join('share', package_name, 'config'),
            glob('config/*.yaml')
        ),
        (
            os.path.join('share', package_name, 'launch'),
            glob('launch/*.launch.py')
        ),
    ],
    install_requires=['setuptools', 'PyYAML'],
    zip_safe=True,
    maintainer='user',
    maintainer_email='user@example.com',
    description='TurtleBot3 Nav2 waypoint and trajectory PyQt GUI',
    license='Apache-2.0',
    entry_points={
        'console_scripts': [
            'nav2_waypoint_gui = tb3_nav2_pyqt_gui.nav2_waypoint_gui:main',
        ],
    },
)

9. launch 파일 작성

launch/nav2_waypoint_gui.launch.py 파일을 작성합니다.

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    yaml_file = LaunchConfiguration('yaml_file')

    default_yaml_file = PathJoinSubstitution([
        FindPackageShare('tb3_nav2_pyqt_gui'),
        'config',
        'waypoints.yaml'
    ])

    return LaunchDescription([
        DeclareLaunchArgument(
            'yaml_file',
            default_value=default_yaml_file,
            description='Waypoint and trajectory YAML file path'
        ),

        Node(
            package='tb3_nav2_pyqt_gui',
            executable='nav2_waypoint_gui',
            name='nav2_waypoint_gui',
            output='screen',
            arguments=['--yaml', yaml_file],
        ),
    ])

10. 빌드

패키지를 빌드합니다.

cd ~/pyqt_ws

colcon build --packages-select tb3_nav2_pyqt_gui

source install/setup.bash

11. 로봇에서 bringup 실행

TurtleBot3 Burger에 SSH로 접속합니다.

ssh sjyong@192.168.210.12

로봇에서 bringup을 실행합니다.

ros2 launch turtlebot3_bringup robot.launch.py

12. 원격 PC에서 Nav2 실행

원격 PC에서 지도 파일을 사용해 Nav2를 실행합니다.

source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
export TURTLEBOT3_MODEL=burger

ros2 launch turtlebot3_navigation2 navigation2.launch.py \
  use_sim_time:=False \
  map:=/home/sjyong/maps/my_map.yaml

Nav2가 정상 실행되면 RViz에서 로봇 위치가 지도 위에 표시되어야 합니다. AMCL 초기 위치가 맞지 않으면 2D Pose Estimate로 초기 위치를 맞춘 뒤 사용해야 합니다.

13. GUI 실행

원격 PC에서 GUI를 실행하는 것을 권장합니다.

source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash

ros2 launch tb3_nav2_pyqt_gui nav2_waypoint_gui.launch.py

다른 YAML 파일을 직접 지정하려면 다음처럼 실행합니다.

ros2 launch tb3_nav2_pyqt_gui nav2_waypoint_gui.launch.py \
  yaml_file:=/home/sjyong/waypoints.yaml

또는 노드를 직접 실행할 수도 있습니다.

ros2 run tb3_nav2_pyqt_gui nav2_waypoint_gui \
  --yaml /home/사용자/waypoints.yaml

14. 동작 확인 명령

Nav2 action server가 살아 있는지 확인합니다.

ros2 action list | grep navigate
ros2 action list | grep follow

정상이라면 보통 아래 액션이 보여야 합니다.

/navigate_to_pose
/follow_waypoints

action 타입도 확인할 수 있습니다.

ros2 action info /navigate_to_pose
ros2 action info /follow_waypoints


Leave a Comment