[Gazebo, ROS2] Python Package Structure for Simulation Code

이 글은 1부: ROS2, Gazebo, PX4 설치와 연결 구조, 2부: Ubuntu 자동 설치 Shell Script에 이어지는 Gazebo Python 코드 관리 방법이다.

Gazebo를 Python으로 다룬다고 할 때 선택지는 두 가지다.

방식	설명
Gazebo Transport를 직접 사용	Gazebo native topic을 직접 다룸
`ros_gz_bridge`를 통해 ROS2 topic으로 변환 후 `rclpy` 사용	ROS2 Python node로 관리

처음에는 두 번째 방식이 관리하기 쉽다. ROS2 package, launch file, parameter, logging, topic echo, rosbag 같은 도구를 그대로 쓸 수 있기 때문이다.

Gazebo 연동 Python 코드는 Gazebo API를 바로 붙잡기보다, ros_gz_bridge로 ROS2 topic화한 뒤 rclpy package로 관리하는 편이 실전에서 유지보수하기 좋다.

목표 구조

우리가 만들 구조는 다음과 같다.

Gazebo
  <-> ros_gz_bridge
  <-> ROS2 topics
  <-> Python nodes

PX4 SITL까지 함께 쓰는 경우에는 다음처럼 나뉜다.

Gazebo <-> PX4 <-> Micro XRCE-DDS Agent <-> ROS2 /fmu topics
Gazebo <-> ros_gz_bridge <-> ROS2 /clock, /imu, /model topics

즉 PX4 제어는 /fmu/* topic을 보고, Gazebo 자체 정보는 bridge된 Gazebo topic을 본다.

Package 만들기

ROS2 workspace로 이동한다.

cd ~/px4_ros2_ws/src

Python package를 만든다.

ros2 pkg create gazebo_sim_tools \
  --build-type ament_python \
  --dependencies rclpy sensor_msgs nav_msgs rosgraph_msgs geometry_msgs

생성된 구조는 대략 다음과 같다.

gazebo_sim_tools/
  package.xml
  setup.py
  setup.cfg
  resource/
    gazebo_sim_tools
  gazebo_sim_tools/
    __init__.py

여기에 launch, config, scripts 성격의 파일을 추가한다.

gazebo_sim_tools/
  config/
    bridge.yaml
  launch/
    gazebo_bridge.launch.py
  gazebo_sim_tools/
    __init__.py
    clock_monitor.py
    imu_monitor.py
    sim_health_node.py

ROS2 Python package에서는 node 코드는 package 내부에 두고, 실행 조합은 launch file, topic mapping은 YAML config로 분리한다.

ros_gz_bridge 설정 파일

Gazebo topic을 ROS2 topic으로 가져오기 위해 config/bridge.yaml을 만든다.

- ros_topic_name: "/clock"
  gz_topic_name: "/clock"
  ros_type_name: "rosgraph_msgs/msg/Clock"
  gz_type_name: "gz.msgs.Clock"
  direction: "GZ_TO_ROS"

- ros_topic_name: "/gazebo/imu"
  gz_topic_name: "/world/default/model/x500/link/base_link/sensor/imu_sensor/imu"
  ros_type_name: "sensor_msgs/msg/Imu"
  gz_type_name: "gz.msgs.IMU"
  direction: "GZ_TO_ROS"

여기서 gz_topic_name은 환경마다 다를 수 있다. 실제 topic은 Gazebo 실행 후 확인한다.

gz topic -l

PX4 X500 model의 sensor topic 이름은 PX4/Gazebo version에 따라 조금 달라질 수 있으므로, gz topic -l | grep imu로 찾는 것이 안전하다.

gz topic -l | grep imu

Bridge Launch File

launch/gazebo_bridge.launch.py를 만든다.

from pathlib import Path

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource


def generate_launch_description():
    package_share = Path(get_package_share_directory("gazebo_sim_tools"))
    bridge_share = Path(get_package_share_directory("ros_gz_bridge"))
    bridge_config = package_share / "config" / "bridge.yaml"

    bridge = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            str(bridge_share / "launch" / "ros_gz_bridge.launch.py")
        ),
        launch_arguments={
            "bridge_name": "gazebo_bridge",
            "config_file": str(bridge_config),
        }.items(),
    )

    return LaunchDescription([bridge])

이 방식은 ros_gz_bridge가 제공하는 launch file을 그대로 포함한다. 사용 가능한 launch argument는 다음 명령으로 확인한다.

ros2 launch ros_gz_bridge ros_gz_bridge.launch.py --show-args

직접 실행할 때는 다음처럼 한다.

ros2 launch ros_gz_bridge ros_gz_bridge.launch.py \
  bridge_name:=gazebo_bridge \
  config_file:=~/px4_ros2_ws/src/gazebo_sim_tools/config/bridge.yaml

Python Node 1: Clock Monitor

시뮬레이션 시간이 제대로 흐르는지 확인하는 node부터 만든다. gazebo_sim_tools/clock_monitor.py:

import rclpy
from rclpy.node import Node
from rosgraph_msgs.msg import Clock


class ClockMonitor(Node):
    def __init__(self) -> None:
        super().__init__("clock_monitor")
        self.last_sec = None
        self.create_subscription(Clock, "/clock", self.on_clock, 10)

    def on_clock(self, msg: Clock) -> None:
        sec = msg.clock.sec
        if self.last_sec != sec:
            self.last_sec = sec
            self.get_logger().info(f"sim time: {sec}s")


def main() -> None:
    rclpy.init()
    node = ClockMonitor()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == "__main__":
    main()

이 node는 복잡한 기능은 없지만 가장 먼저 확인해야 하는 것을 확인한다. /clock이 들어온다는 것은 Gazebo와 ROS2 bridge가 살아 있다는 뜻이다.

Python Node 2: IMU Monitor

Gazebo IMU topic을 ROS2로 bridge했다면 Python에서 sensor_msgs/msg/Imu로 받을 수 있다. gazebo_sim_tools/imu_monitor.py:

import math

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu


class ImuMonitor(Node):
    def __init__(self) -> None:
        super().__init__("imu_monitor")
        self.create_subscription(Imu, "/gazebo/imu", self.on_imu, 10)

    def on_imu(self, msg: Imu) -> None:
        ax = msg.linear_acceleration.x
        ay = msg.linear_acceleration.y
        az = msg.linear_acceleration.z
        norm = math.sqrt(ax * ax + ay * ay + az * az)
        self.get_logger().info(f"acc norm: {norm:.3f} m/s^2")


def main() -> None:
    rclpy.init()
    node = ImuMonitor()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == "__main__":
    main()

이 코드는 Gazebo topic을 바로 읽는 것이 아니라 bridge된 ROS2 topic을 읽는다. 그래서 ros2 topic echo /gazebo/imu로도 같은 데이터를 확인할 수 있다.

Python Node 3: Simulation Health Node

실제 프로젝트에서는 topic 하나만 보는 것보다 simulation 상태를 종합해서 확인하는 node가 있으면 편하다. 예를 들어 /clock과 /gazebo/imu가 일정 시간 안에 들어오는지 확인한다.

gazebo_sim_tools/sim_health_node.py:

import time

import rclpy
from rclpy.node import Node
from rosgraph_msgs.msg import Clock
from sensor_msgs.msg import Imu


class SimHealthNode(Node):
    def __init__(self) -> None:
        super().__init__("sim_health_node")
        self.last_clock_time = None
        self.last_imu_wall_time = None

        self.create_subscription(Clock, "/clock", self.on_clock, 10)
        self.create_subscription(Imu, "/gazebo/imu", self.on_imu, 10)
        self.create_timer(1.0, self.check_health)

    def on_clock(self, msg: Clock) -> None:
        self.last_clock_time = msg.clock.sec + msg.clock.nanosec * 1e-9

    def on_imu(self, _msg: Imu) -> None:
        self.last_imu_wall_time = time.monotonic()

    def check_health(self) -> None:
        now = time.monotonic()

        if self.last_clock_time is None:
            self.get_logger().warn("no /clock received")
        else:
            self.get_logger().info(f"sim clock: {self.last_clock_time:.2f}s")

        if self.last_imu_wall_time is None:
            self.get_logger().warn("no /gazebo/imu received")
            return

        age = now - self.last_imu_wall_time
        if age > 2.0:
            self.get_logger().warn(f"/gazebo/imu stale: {age:.1f}s")
        else:
            self.get_logger().info("/gazebo/imu OK")


def main() -> None:
    rclpy.init()
    node = SimHealthNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == "__main__":
    main()

시뮬레이션 코드는 기능 코드보다 먼저 health check node를 만들어두면 디버깅 시간이 크게 줄어든다.

setup.py에 실행 파일 등록

setup.py에서 console script를 등록한다.

from setuptools import find_packages, setup

package_name = "gazebo_sim_tools"

setup(
    name=package_name,
    version="0.0.0",
    packages=find_packages(exclude=["test"]),
    data_files=[
        ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
        (f"share/{package_name}", ["package.xml"]),
        (f"share/{package_name}/launch", ["launch/gazebo_bridge.launch.py"]),
        (f"share/{package_name}/config", ["config/bridge.yaml"]),
    ],
    install_requires=["setuptools"],
    zip_safe=True,
    maintainer="br4c3",
    maintainer_email="br4c3@example.com",
    description="Gazebo ROS2 simulation utilities",
    license="MIT",
    entry_points={
        "console_scripts": [
            "clock_monitor = gazebo_sim_tools.clock_monitor:main",
            "imu_monitor = gazebo_sim_tools.imu_monitor:main",
            "sim_health = gazebo_sim_tools.sim_health_node:main",
        ],
    },
)

data_files에 launch와 config를 넣어야 install space에서도 launch file과 YAML을 찾을 수 있다.

Build와 실행

workspace root에서 build한다.

cd ~/px4_ros2_ws
source /opt/ros/humble/setup.bash
colcon build --symlink-install
source install/local_setup.bash

Gazebo와 bridge를 먼저 실행한다.

Terminal 1:

cd ~/PX4-Autopilot
make px4_sitl gz_x500

Terminal 2:

source /opt/ros/humble/setup.bash
source ~/px4_ros2_ws/install/local_setup.bash
ros2 launch ros_gz_bridge ros_gz_bridge.launch.py \
  bridge_name:=gazebo_bridge \
  config_file:=~/px4_ros2_ws/src/gazebo_sim_tools/config/bridge.yaml

Terminal 3:

source /opt/ros/humble/setup.bash
source ~/px4_ros2_ws/install/local_setup.bash
ros2 run gazebo_sim_tools sim_health

topic이 잘 들어오는지 확인한다.

ros2 topic list | grep gazebo
ros2 topic echo /gazebo/imu

Launch File로 Node까지 같이 실행

매번 bridge와 node를 따로 실행하기 귀찮다면 launch file에 함께 넣는다. launch/sim_tools.launch.py:

from pathlib import Path

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node


def generate_launch_description():
    package_share = Path(get_package_share_directory("gazebo_sim_tools"))
    bridge_share = Path(get_package_share_directory("ros_gz_bridge"))
    bridge_config = package_share / "config" / "bridge.yaml"

    bridge = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            str(bridge_share / "launch" / "ros_gz_bridge.launch.py")
        ),
        launch_arguments={
            "bridge_name": "gazebo_bridge",
            "config_file": str(bridge_config),
        }.items(),
    )

    sim_health = Node(
        package="gazebo_sim_tools",
        executable="sim_health",
        name="sim_health",
        output="screen",
    )

    return LaunchDescription([
        bridge,
        sim_health,
    ])

이 파일도 setup.py의 data_files에 추가한다.

(f"share/{package_name}/launch", [
    "launch/gazebo_bridge.launch.py",
    "launch/sim_tools.launch.py",
]),

다시 build 후 실행한다.

cd ~/px4_ros2_ws
colcon build --symlink-install
source install/local_setup.bash
ros2 launch gazebo_sim_tools sim_tools.launch.py

코드 관리 원칙

Gazebo simulation code는 다음 기준으로 나누면 관리가 편하다.

파일 종류	위치	역할
Python node	`gazebo_sim_tools/*.py`	topic subscribe/publish, 상태 판단
Launch file	`launch/*.launch.py`	여러 node와 bridge 실행 조합
Bridge config	`config/*.yaml`	Gazebo topic과 ROS2 topic mapping
World/model file	별도 `worlds/`, `models/` 또는 PX4 쪽	simulation scene
반복 실행 shell	`~/drone_dev/scripts/`	terminal 실행 자동화

Python node 안에 topic 이름을 모두 박아두면 나중에 바꾸기 어렵다. 처음에는 상수로 써도 되지만, 자주 바뀌는 값은 parameter로 빼는 것이 좋다.

예를 들어 IMU topic을 parameter로 받는다.

self.declare_parameter("imu_topic", "/gazebo/imu")
imu_topic = self.get_parameter("imu_topic").value
self.create_subscription(Imu, imu_topic, self.on_imu, 10)

launch file에서 값을 넘긴다.

Node(
    package="gazebo_sim_tools",
    executable="imu_monitor",
    parameters=[{"imu_topic": "/gazebo/imu"}],
)

PX4 제어 코드와 Gazebo 관측 코드는 분리한다

PX4 SITL에서는 Gazebo가 물리 시뮬레이션을 하고, PX4가 autopilot을 담당한다. 따라서 Python node도 역할을 나누는 것이 좋다.

px4_control_nodes/
  offboard_control.py
  vehicle_status_monitor.py
  trajectory_sender.py

gazebo_sim_tools/
  sim_health_node.py
  imu_monitor.py
  clock_monitor.py

px4_control_nodes는 /fmu/in/*, /fmu/out/*를 다룬다. gazebo_sim_tools는 /clock, /gazebo/*, /world/* 같은 Gazebo bridge topic을 다룬다.

이렇게 나누면 나중에 실제 기체로 옮길 때도 편하다. 실기체에는 Gazebo topic이 없지만 PX4 topic은 남기 때문이다.

다음 단계

여기까지 되면 Gazebo와 ROS2 Python code를 관리할 기본 구조가 잡힌다. 다음 단계는 PX4 offboard control node를 만들어 /fmu/in/trajectory_setpoint, /fmu/in/offboard_control_mode, /fmu/in/vehicle_command를 publish하는 것이다.

PX4 파라미터를 ROS2에서 바꾸는 내용은 PX4 PID 파라미터 조절 글에 따로 정리했다.