[rclpy] PointCloud2 Publisher와 Subscriber 만들기!

Publisher

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2, PointField
import numpy as np
import struct
from std_msgs.msg import Header


class PointCloudPublisher(Node):
    def __init__(self):
        super().__init__('pointcloud_publisher')
        self.publisher_ = self.create_publisher(PointCloud2, 'pointcloud', 10)
        self.timer = self.create_timer(1.0, self.timer_callback)
        self.i = 0

    def timer_callback(self):
        points = np.random.rand(100, 3)  # 예시 데이터, 실제로는 입력 데이터를 사용
        msg = self.create_pointcloud2_message(points)
        self.publisher_.publish(msg)
        self.get_logger().info('Publishing PointCloud2')

    def create_pointcloud2_message(self, points):
        header = Header(frame_id="map")
        header.stamp = self.get_clock().now().to_msg()

        fields = [PointField(name='x', offset=0, datatype=PointField.FLOAT32,
                             count=1),
                  PointField(name='y', offset=4, datatype=PointField.FLOAT32,
                             count=1),
                  PointField(name='z', offset=8, datatype=PointField.FLOAT32,
                             count=1)]

        buf = []
        for point in points:
            buf.append(struct.pack('fff', point[0], point[1], point[2]))

        return PointCloud2(
            header=header,
            height=1,
            width=len(points),
            is_dense=False,
            is_bigendian=False,
            fields=fields,
            point_step=12,  # FLOAT32 (4 bytes) * 3 (x, y, z)
            row_step=12 * len(points),
            data=b''.join(buf)
        )


def main(args=None):
    rclpy.init(args=args)
    pointcloud_publisher = PointCloudPublisher()
    rclpy.spin(pointcloud_publisher)
    pointcloud_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

struct.pack('fff', point[0], point[1], point[2])

• struct.pack 함수는 Python 값들을 바이트 객체로 패킹하는 데 사용
  • 패킹?
    • 하나 이상의 데이터 값을 바이트 또는 바이너리 형태로 변환하는 과정
    • 데이터를 저장하거나 전송하기 위해 특정 형식으로 조직화하는 것을 의미
• 이 함수는 포맷 문자열을 기반으로 지정된 형식에 맞게 하나 이상의 값을 바이트로 변환
  • 포맷 문자열
    • 데이터를 어떤 형식으로 패킹할지를 지정하는 문자열
    • 포맷 문자열은 데이터 타입과 크기를 나타내는 문자 코드를 포함
    • 예를 들어, 'f'는 부동소수점 실수를 나타내고, 'i'는 정수를 나타냄
    • 포맷 문자열은 패킹하려는 각 데이터 값의 타입을 순서대로 명시하여, 해당 데이터를 바이너리 형식으로 정확히 어떻게 변환할지를 결정
• struct.pack 함수의 사용은 바이너리 데이터를 처리할 때 매우 유용하며,
  • 특히 네트워킹, 파일 I/O, 바이너리 데이터 포맷의 변환 등 다양한 저수준 바이너리 데이터 처리 작업에 필수적
• struct.pack의 첫 번째 인자는 포맷 문자열이며, 이 문자열은 패킹할 데이터의 타입과 배열을 명시
• 포맷 문자열 'fff'는 세 개의 실수(float) 값을 나타냄
• 여기서 각각의 'f'는 C 타입의 float에 해당하는 4바이트 크기의 부동소수점 실수를 의미
• 따라서 'fff' 포맷 문자열은, 세 개의 부동소수점 실수를 연속적으로 패킹하겠다는 것을 나타냄
• 코드 struct.pack('fff', point[0], point[1], point[2])는 따라서 point 배열의 첫 번째, 두 번째, 세 번째 요소(각각 x, y, z 좌표를 나타내는 실수)를 하나의 바이트 객체로 패킹하는 과정
• 이렇게 패킹된 바이트 객체는 포인트 클라우드 데이터의 일부로서 사용되며, 각 포인트는 12바이트(4바이트 * 3) 크기를 갖게 됩니다.
• 이 함수의 output은 bytes 타입입니다.
• 최종적으로 이 바이트 객체들은 전체 포인트 클라우드 메시지의 data 필드에 순서대로 연결되어 포인트 클라우드 데이터를 구성

data=b''.join(buf)

• buf 리스트에 저장된 여러 바이트 객체들을 하나의 바이트 객체로 결합하는 과정
• 여기서 b''는 빈 바이트 객체를 의미
• join 메서드는 이 빈 바이트 객체를 사용하여 buf 리스트에 있는 모든 바이트 객체들을 순서대로 연결
• 결과적으로, 모든 바이트 객체들이 하나의 바이트 스트림으로 결합되어 data 변수에 저장
• 간단한 예로, 만약 buf 리스트에 [b'\x01\x02', b'\x03\x04']와 같은 두 바이트 객체가 있다면,
• b''.join(buf)의 결과는 b'\x01\x02\x03\x04'가 됩니다.
• 이 예에서는 두 바이트 객체가 하나의 바이트 객체로 결합되어 순서대로 이어진 것을 볼 수 있습니다.

b'\x01\x02' 와 같은 바이트 객체 해석법

• 바이트 객체 b'\x01\x02'는 바이너리 데이터를 나타내는 Python의 표현
• 여기서 b 접두사는 뒤이어 오는 문자열이 바이트 문자열임을 나타냄
• \x는 16진수(hexadecimal) 값을 나타내는 이스케이프 시퀀스
• b'\x01\x02'에서:
  • \x01은 16진수로 01을 의미하며, 10진수로는 1입니다.
  • \x02은 16진수로 02를 의미하며, 10진수로는 2입니다.
• 따라서 이 바이트 문자열은 두 개의 바이트로 구성되어 있으며, 첫 번째 바이트는 값이 1이고, 두 번째 바이트는 값이 2인 바이너리 데이터
• 바이트 객체 내의 개별 바이트는 0에서 255(16진수로는 0x00에서 0xFF) 사이의 값을 가질 수 있으며, 이는 8비트의 데이터를 나타냅니다.
• 1바이트는 8비트니까, 2^8 = 256개 숫자를 표현할 수 있는데, 256 = 16^2 이므로,
  • 1바이트는 16진수 2자리로 표현하면 편하다.

0x와 \x 접두사 차이

• 결론적으로,
  • 0x는 주로 숫자와 함께 16진수 값을 나타내는 데 사용
  • \x는 바이트 문자열 내에서 특정 바이트 값을 직접 표현하는 데 사용
• 0x 사용 예: 변수에 16진수 값을 할당하거나, 16진수 형태의 상수를 코드에 직접 입력할 때 사용합니다. 예: color = 0xFF00FF (RGB에서 마젠타 색상)
  \x 사용 예: 바이너리 데이터를 문자열 형태로 작성할 때, 특정 바이트 값을 직접 포함시키기 위해 사용합니다. 예: data = b'\x00\xFF' (0과 255 값을 갖는 바이트 데이터)