본문으로 건너뛰기

센서 수신 노드 실습

센서가 없는 환경에서도 센서 수신 노드를 실습할 수 있도록 IMU 데이터를 시뮬레이션합니다. 이 실습에서는 imu_data_simulation 패키지가 발행하는 /imu/data 토픽을 확인하고, 직접 만든 Python 노드에서 sensor_data QoS로 IMU 메시지를 수신합니다.

참고 자료

IMU 시뮬레이션 패키지는 mechasolution/imu_data_simulation을 사용합니다. 구독 노드는 ROS 2 Jazzy 공식 문서의 Python publisher/subscriber 예제를 바탕으로 작성합니다.

IMU 시뮬레이션 패키지 설치​

워크스페이스의 src 폴더에 시뮬레이션 패키지를 복제합니다.

$ cd ~/ros2_ws/src
~/ros2_ws/src$ git clone https://github.com/mechasolution/imu_data_simulation.git

패키지를 빌드하고 환경을 적용합니다.

$ cd ~/ros2_ws
~/ros2_ws$ colcon build --packages-select imu_data_simulation
~/ros2_ws$ source install/setup.bash

IMU 시뮬레이션 노드를 실행합니다.

$ ros2 run imu_data_simulation moving_imu_publisher

이 노드는 /imu/data 토픽으로 sensor_msgs/msg/Imu 메시지를 발행합니다. orientation 값이 계속 변하므로 실제 IMU가 움직이는 것처럼 수신 실습을 할 수 있습니다.

토픽 확인​

새 터미널을 열고 현재 발행 중인 토픽 목록을 확인합니다.

$ ros2 topic list
/imu/data
/parameter_events
/rosout

/imu/data 토픽의 메시지 타입과 QoS를 확인합니다.

$ ros2 topic info -v /imu/data
Type: sensor_msgs/msg/Imu

Publisher count: 1

Node name: moving_imu_publisher
Node namespace: /
Topic type: sensor_msgs/msg/Imu
Topic type hash: RIHS01_7d9a00ff131080897a5ec7e26e315954b8eae3353c3f995c55faf71574000b5b
Endpoint type: PUBLISHER
GID: 01.0f.aa.b1.e6.d6.c8.d9.00.00.00.00.00.00.13.03
QoS profile:
  Reliability: BEST_EFFORT
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Subscription count: 0

Reliability가 BEST_EFFORT로 표시됩니다. ROS 2의 sensor_data QoS 프리셋은 센서 데이터처럼 빠르게 들어오는 토픽에서 최신 데이터를 받는 데 적합하며, Best Effort 신뢰성 정책을 사용합니다.

IMU 메시지도 직접 확인할 수 있습니다.

$ ros2 topic echo /imu/data --qos-profile sensor_data --once
header:
  stamp:
    sec: 1783935022
    nanosec: 773792547
  frame_id: imu_link
orientation:
  x: 0.0
  y: 0.0
  z: 0.5821055318839353
  w: 0.8131132453417056
# (생략)

수신 패키지 생성​

이제 /imu/data 토픽을 구독하는 패키지를 만듭니다.

$ cd ~/ros2_ws/src
~/ros2_ws/src$ ros2 pkg create sensor_subscribe_tutorial \
  --build-type ament_python \
  --dependencies rclpy sensor_msgs

패키지 안의 Python 모듈 폴더로 이동합니다.

$ cd ~/ros2_ws/src/sensor_subscribe_tutorial/sensor_subscribe_tutorial

공식 예제 코드 가져오기​

ROS 2 Jazzy 공식 문서의 Python publisher/subscriber 예제 중 minimal subscriber 코드를 가져옵니다. 이번 실습은 센서 데이터를 수신하는 노드를 만드는 것이므로 publisher가 아니라 subscriber 예제를 수정합니다.

$ wget https://raw.githubusercontent.com/ros2/examples/jazzy/rclpy/topics/minimal_subscriber/examples_rclpy_minimal_subscriber/subscriber_member_function.py
$ mv subscriber_member_function.py imu_subscriber.py

가져온 예제는 std_msgs/msg/String 타입의 topic 토픽을 구독합니다. 우리는 sensor_msgs/msg/Imu 타입의 /imu/data 토픽을 sensor_data QoS로 구독하도록 수정합니다.

수신 노드 코드 수정​

imu_subscriber.py 파일을 아래처럼 수정합니다.

imu_subscriber.py
# Copyright 2016 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

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


class ImuSubscriber(Node):

    def __init__(self):
        super().__init__('imu_subscriber')
        self.subscription = self.create_subscription(
            Imu,
            '/imu/data',
            self.imu_callback,
            qos_profile_sensor_data
        )
        self.subscription  # prevent unused variable warning

    def imu_callback(self, msg):
        self.get_logger().info(
            'orientation: '
            f'x={msg.orientation.x:.3f}, '
            f'y={msg.orientation.y:.3f}, '
            f'z={msg.orientation.z:.3f}, '
            f'w={msg.orientation.w:.3f} | '
            'angular_velocity: '
            f'z={msg.angular_velocity.z:.3f}'
        )


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

    imu_subscriber = ImuSubscriber()

    rclpy.spin(imu_subscriber)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    imu_subscriber.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

공식 예제에서 바꾼 핵심 부분은 다음과 같습니다.

  • String 대신 Imu 메시지 타입을 import합니다.
  • MinimalSubscriber 클래스 이름을 ImuSubscriber로 바꿉니다.
  • 노드 이름을 minimal_subscriber에서 imu_subscriber로 바꿉니다.
  • create_subscription()의 메시지 타입을 Imu로 바꿉니다.
  • 토픽 이름을 topic에서 /imu/data로 바꿉니다.
  • QoS 인수를 10에서 qos_profile_sensor_data로 바꿉니다.
  • 콜백 함수에서 IMU의 orientation과 angular velocity를 출력합니다.

수정한 코드 구문별 설명​

먼저 메시지 타입과 QoS 설정을 import합니다.

from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import Imu

qos_profile_sensor_data는 센서 데이터 수신에 자주 사용하는 QoS 프리셋입니다. IMU처럼 빠르게 들어오는 데이터는 모든 과거 데이터를 놓치지 않는 것보다 최신 데이터를 빠르게 받는 것이 더 중요합니다. Imu는 /imu/data 토픽에서 사용하는 메시지 타입입니다.

class ImuSubscriber(Node):

ImuSubscriber는 우리가 새로 만든 수신 노드 클래스입니다. 공식 예제의 MinimalSubscriber는 문자열 예제용 이름이므로, 이 실습에서는 IMU 데이터를 받는다는 의미가 드러나도록 이름을 바꿉니다. 괄호 안의 Node는 이 클래스가 ROS 2 노드 기능을 상속받는다는 뜻입니다.

super().__init__('imu_subscriber')

super().__init__()은 부모 클래스인 Node를 초기화합니다. 괄호 안의 'imu_subscriber'는 ROS 2 그래프에 표시될 노드 이름입니다. 노드를 실행한 뒤 ros2 node list를 입력하면 이 이름을 확인할 수 있습니다.

self.subscription = self.create_subscription(
    Imu,
    '/imu/data',
    self.imu_callback,
    qos_profile_sensor_data
)

create_subscription()은 토픽을 구독하는 코드를 만듭니다. 첫 번째 인수 Imu는 수신할 메시지 타입입니다. 두 번째 인수 '/imu/data'는 구독할 토픽 이름입니다. 세 번째 인수 self.imu_callback은 메시지가 들어올 때마다 실행할 함수입니다. 네 번째 인수 qos_profile_sensor_data는 이 구독자가 센서 데이터용 QoS로 통신하겠다는 설정입니다.

def imu_callback(self, msg):

콜백 함수는 메시지를 수신했을 때 자동으로 호출됩니다. 여기서 msg에는 방금 수신한 sensor_msgs/msg/Imu 메시지 하나가 들어 있습니다.

msg.orientation.x
msg.orientation.y
msg.orientation.z
msg.orientation.w

orientation은 IMU의 자세를 나타냅니다. ROS 2의 IMU 메시지는 자세를 quaternion 형식의 x, y, z, w 값으로 표현합니다. 이번 시뮬레이션에서는 yaw 방향으로 회전하도록 만들었기 때문에 주로 z와 w 값이 계속 변합니다.

msg.angular_velocity.z

angular_velocity는 각속도입니다. z 값은 z축을 기준으로 얼마나 빠르게 회전하고 있는지를 나타냅니다.

imu_subscriber = ImuSubscriber()
rclpy.spin(imu_subscriber)

ImuSubscriber()는 수신 노드 객체를 만듭니다. rclpy.spin()은 노드가 종료되지 않고 계속 실행되도록 유지합니다. 이 상태에서 /imu/data 메시지가 들어오면 imu_callback()이 반복해서 실행됩니다.

실행 명령 등록​

ros2 run으로 실행할 수 있도록 setup.py의 entry_points를 수정합니다.

$ cd ~/ros2_ws/src/sensor_subscribe_tutorial
setup.py
from setuptools import find_packages, setup

package_name = 'sensor_subscribe_tutorial'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='max',
    maintainer_email='max@nizcorp.kr',
    description='TODO: Package description',
    license='TODO: License declaration',
    extras_require={
        'test': [
            'pytest',
        ],
    },
    entry_points={
        'console_scripts': [
            'imu_subscriber = sensor_subscribe_tutorial.imu_subscriber:main',
        ],
    },
)

강조된 줄의 왼쪽은 실행 명령 이름이고, 오른쪽은 패키지_모듈.파일이름:main 형식입니다. 따라서 위 설정을 추가하면 다음 명령으로 노드를 실행할 수 있습니다.

$ ros2 run sensor_subscribe_tutorial imu_subscriber

빌드​

워크스페이스 루트로 이동해 패키지를 빌드합니다.

$ cd ~/ros2_ws
~/ros2_ws$ colcon build --packages-select sensor_subscribe_tutorial
~/ros2_ws$ source install/setup.bash

실행​

터미널 1에서 IMU 시뮬레이션 노드를 실행합니다.

$ ros2 run imu_data_simulation moving_imu_publisher

터미널 2에서 직접 만든 수신 노드를 실행합니다.

$ ros2 run sensor_subscribe_tutorial imu_subscriber

다음처럼 orientation 값이 계속 출력되면 /imu/data 토픽을 정상적으로 수신한 것입니다.

[INFO] [1783932019.782938823] [imu_subscriber]: orientation: x=0.000, y=0.000, z=-0.820, w=-0.572 | angular_velocity: z=0.500
[INFO] [1783932019.822648365] [imu_subscriber]: orientation: x=0.000, y=0.000, z=-0.827, w=-0.562 | angular_velocity: z=0.500
[INFO] [1783932019.873188948] [imu_subscriber]: orientation: x=0.000, y=0.000, z=-0.834, w=-0.551 | angular_velocity: z=0.500