659.72 tf2_sensor_msgs를 이용한 센서 데이터 변환

1. 개요

tf2_sensor_msgs는 ROS2의 sensor_msgs 패키지에 정의된 센서 데이터 메시지를 TF2 프레임워크와 연동하여 좌표 변환을 수행할 수 있도록 하는 확장 패키지이다. TF2의 타입 확장 아키텍처에 따라, sensor_msgs 메시지 타입에 대한 tf2::doTransform() 함수의 템플릿 특수화를 제공함으로써, 센서 데이터를 임의의 좌표 프레임으로 효율적으로 변환할 수 있게 한다. 주된 대상 메시지 타입은 sensor_msgs::msg::PointCloud2이며, 대규모 포인트 클라우드의 고성능 좌표 변환을 핵심 기능으로 제공한다.

2. tf2_sensor_msgs의 아키텍처

2.1 TF2 확장 패키지 위치

TF2 생태계에서 tf2_sensor_msgs는 다음과 같이 위치한다.

tf2 (핵심 변환 라이브러리)
├── tf2_ros (ROS2 인터페이스)
├── tf2_geometry_msgs (geometry_msgs 변환)
├── tf2_sensor_msgs (sensor_msgs 변환)
├── tf2_eigen (Eigen 연동)
└── tf2_kdl (KDL 연동)

각 확장 패키지는 독립적으로 설치 및 사용 가능하며, 필요한 메시지 타입의 변환만 선택적으로 활성화할 수 있다.

2.2 패키지 구성

3. 의존성 설정

3.1 C++ (CMakeLists.txt)

find_package(tf2_sensor_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(sensor_msgs REQUIRED)

ament_target_dependencies(${PROJECT_NAME}
  tf2
  tf2_ros
  tf2_sensor_msgs
  sensor_msgs
)

3.2 package.xml

<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_sensor_msgs</depend>
<depend>sensor_msgs</depend>

4. 지원 메시지 타입

4.1 PointCloud2

tf2_sensor_msgs의 핵심 기능은 sensor_msgs::msg::PointCloud2에 대한 좌표 변환이다.

PointCloud2 변환은 다음 단계로 수행된다.

1. TransformStamped 메시지에서 4 \times 4 동차 변환 행렬을 구성한다
2. 포인트 클라우드의 각 포인트에 대해 좌표 변환을 적용한다
3. 출력 메시지의 헤더를 대상 프레임으로 갱신한다

내부적으로 Eigen 라이브러리를 활용한 행렬 연산이 사용되어, 대규모 포인트 클라우드의 효율적인 변환이 가능하다.

5. C++에서의 사용법

5.1 헤더 포함

#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>

이 헤더의 포함이 PointCloud2에 대한 doTransform() 특수화를 활성화한다.

5.2 doTransform()을 이용한 변환

#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

sensor_msgs::msg::PointCloud2 cloud_in;
// cloud_in은 센서 콜백에서 수신

sensor_msgs::msg::PointCloud2 cloud_out;

try {
  auto transform = tf_buffer->lookupTransform(
    "base_link",
    cloud_in.header.frame_id,
    cloud_in.header.stamp);
  
  tf2::doTransform(cloud_in, cloud_out, transform);
  
  // cloud_out.header.frame_id == "base_link"
} catch (const tf2::TransformException & ex) {
  RCLCPP_ERROR(node->get_logger(), "%s", ex.what());
}

5.3 Buffer::transform()을 이용한 간편 변환

try {
  sensor_msgs::msg::PointCloud2 cloud_out;
  tf_buffer->transform(cloud_in, cloud_out, "base_link");
} catch (const tf2::TransformException & ex) {
  RCLCPP_ERROR(node->get_logger(), "%s", ex.what());
}

5.4 전체 노드 예제

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>

class CloudTransformNode : public rclcpp::Node
{
public:
  CloudTransformNode() : Node("cloud_transform")
  {
    tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(
      *tf_buffer_);
    
    cloud_sub_ = this->create_subscription<
      sensor_msgs::msg::PointCloud2>(
      "/camera/depth/points", 10,
      std::bind(&CloudTransformNode::cloud_callback, this,
                std::placeholders::_1));
    
    cloud_pub_ = this->create_publisher<
      sensor_msgs::msg::PointCloud2>(
      "/cloud_in_base", 10);
  }

private:
  void cloud_callback(
    const sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg)
  {
    try {
      sensor_msgs::msg::PointCloud2 cloud_out;
      tf_buffer_->transform(*cloud_msg, cloud_out, "base_link");
      cloud_pub_->publish(cloud_out);
    } catch (const tf2::TransformException & ex) {
      RCLCPP_WARN_THROTTLE(
        this->get_logger(), *this->get_clock(), 1000,
        "변환 실패: %s", ex.what());
    }
  }

  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_sub_;
  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_pub_;
};

6. Python에서의 사용법

6.1 모듈 임포트

import tf2_sensor_msgs

6.2 변환 수행

from sensor_msgs.msg import PointCloud2
import tf2_sensor_msgs

def cloud_callback(cloud_msg):
    try:
        transform = tf_buffer.lookup_transform(
            'base_link',
            cloud_msg.header.frame_id,
            cloud_msg.header.stamp)
        
        cloud_out = tf2_sensor_msgs.do_transform_cloud(
            cloud_msg, transform)
        
        cloud_pub.publish(cloud_out)
    except TransformException as ex:
        node.get_logger().warn(f'{ex}')

6.3 Buffer.transform()을 이용한 간편 변환

import tf2_sensor_msgs  # 반드시 import하여 doTransform 등록

try:
    cloud_out = tf_buffer.transform(cloud_msg, 'base_link')
except TransformException as ex:
    node.get_logger().error(f'{ex}')

7. 내부 구현의 성능 특성

7.1 Eigen 기반 행렬 연산

tf2_sensor_msgs는 내부적으로 TransformStamped에서 Eigen의 Eigen::Isometry3f 또는 Eigen::Matrix4f 변환 행렬을 구성하고, 포인트 데이터를 Eigen 행렬로 매핑(mapping)하여 일괄 변환을 수행한다. Eigen의 Map 기능을 통해 PointCloud2의 바이너리 데이터를 복사 없이 직접 참조할 수 있어 메모리 효율이 뛰어나다.

7.2 변환되는 필드

tf2_sensor_msgs의 PointCloud2 변환은 기본적으로 x, y, z 필드만 변환한다. 법선 벡터(normal_x, normal_y, normal_z)가 포인트 클라우드에 포함된 경우, 이들은 자동으로 변환되지 않으므로 별도의 처리가 필요하다.

7.3 메모리 관리

변환 결과는 새로운 PointCloud2 메시지에 기록되므로, 입력 메시지와 출력 메시지가 별도의 메모리를 점유한다. 대규모 포인트 클라우드(수십만 포인트 이상)의 경우, 메모리 할당과 해제에 의한 오버헤드가 발생할 수 있다. 반복적인 변환에서는 출력 메시지를 재사용하여 메모리 할당 횟수를 최소화하는 것이 권장된다.

8. tf2_geometry_msgs와의 비교

9. 일반적 오류

9.1 헤더 미포함

C++에서 tf2_sensor_msgs/tf2_sensor_msgs.hpp 헤더를 포함하지 않으면 PointCloud2에 대한 doTransform() 특수화가 인식되지 않아 컴파일 오류가 발생한다.

9.2 빈 포인트 클라우드

포인트 수가 0인 빈 PointCloud2 메시지에 대해 변환를 시도하면, 일부 구현에서 예기치 않은 동작이 발생할 수 있다. 변환 전에 cloud.width * cloud.height > 0을 확인하는 것이 안전하다.

9.3 비표준 필드 배치

포인트 클라우드의 fields 배치가 표준과 다른 경우(예: x, y, z가 처음 3개 필드가 아닌 경우), tf2_sensor_msgs의 변환이 올바르게 동작하지 않을 수 있다. fields 배열에서 x, y, z 필드의 오프셋(offset)을 확인하여 정확한 위치를 파악해야 한다.

10. 참고 문헌

• Open Robotics, “tf2_sensor_msgs,” ROS 2 Documentation, https://docs.ros.org/en/rolling/p/tf2_sensor_msgs/
• Open Robotics, “sensor_msgs,” ROS 2 Documentation, https://docs.ros.org/en/rolling/p/sensor_msgs/
• G. Guennebaud, B. Jacob, et al., “Eigen v3,” https://eigen.tuxfamily.org

버전: 2026-03-26