1296.73 ProcessPointCloud 액션 노드 구현

1. 개요

ProcessPointCloud 액션 노드는 라이다(LiDAR) 또는 깊이 카메라(depth camera)로부터 획득한 3차원 포인트 클라우드(point cloud) 데이터를 처리하여 후속 노드에서 활용할 수 있는 형태로 변환하는 리프 노드이다. 포인트 클라우드 처리는 환경 인식, 장애물 탐지, 3차원 매핑, 지형 분석 등 로봇의 자율 의사 결정에 필수적인 기반 작업이다.

포인트 클라우드 처리는 수만에서 수백만 개의 3차원 점으로 구성된 대용량 데이터를 다루므로, 연산 시간이 수십 밀리초에서 수 초까지 소요될 수 있다. 따라서 BT::StatefulActionNode를 상속하여 비동기 방식으로 구현한다.

2. 포인트 클라우드 데이터 구조

2.1 ROS2 PointCloud2 메시지

ROS2에서 포인트 클라우드 데이터는 sensor_msgs/msg/PointCloud2 메시지 타입으로 전달된다. 이 메시지는 필드(field) 기반의 유연한 데이터 구조를 가지며, 각 점의 좌표(x, y, z)와 추가 속성(반사 강도, 색상, 법선 벡터 등)을 포함할 수 있다.

주요 필드 구조는 다음과 같다.

2.2 PCL 라이브러리와의 연동

Point Cloud Library(PCL)는 포인트 클라우드 처리를 위한 표준 C++ 라이브러리이다. ROS2에서는 pcl_conversions 패키지를 통해 sensor_msgs/msg/PointCloud2와 pcl::PointCloud<T> 간의 변환을 수행한다.

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

// ROS2 메시지를 PCL로 변환
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(
    new pcl::PointCloud<pcl::PointXYZI>);
pcl::fromROSMsg(*ros_cloud_msg, *cloud);

3. 클래스 구조 설계

#include <behaviortree_cpp/action_node.h>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

class ProcessPointCloudAction : public BT::StatefulActionNode
{
public:
    using PointT = pcl::PointXYZI;
    using CloudT = pcl::PointCloud<PointT>;

    ProcessPointCloudAction(
        const std::string& name,
        const BT::NodeConfiguration& config,
        rclcpp::Node::SharedPtr node)
        : BT::StatefulActionNode(name, config),
          node_(node)
    {
    }

    static BT::PortsList providedPorts()
    {
        return {
            BT::InputPort<std::string>("lidar_topic",
                "/lidar/points",
                "라이다 포인트 클라우드 토픽"),
            BT::InputPort<double>("voxel_size", 0.1,
                "복셀 다운샘플링 크기 (미터)"),
            BT::InputPort<double>("range_min", 0.5,
                "최소 거리 필터 (미터)"),
            BT::InputPort<double>("range_max", 100.0,
                "최대 거리 필터 (미터)"),
            BT::InputPort<bool>("remove_ground", true,
                "지면 제거 수행 여부"),
            BT::InputPort<double>("timeout", 5.0,
                "데이터 수신 타임아웃 (초)"),
            BT::OutputPort<std::shared_ptr<
                sensor_msgs::msg::PointCloud2>>(
                "processed_cloud", "처리된 포인트 클라우드"),
            BT::OutputPort<int>("point_count",
                "처리 후 점 개수")
        };
    }

    BT::NodeStatus onStart() override;
    BT::NodeStatus onRunning() override;
    void onHalted() override;

private:
    rclcpp::Node::SharedPtr node_;
    rclcpp::Subscription<
        sensor_msgs::msg::PointCloud2>::SharedPtr
        subscription_;
    sensor_msgs::msg::PointCloud2::SharedPtr raw_cloud_;
    std::atomic<bool> data_received_{false};
    std::atomic<bool> processing_done_{false};
    sensor_msgs::msg::PointCloud2::SharedPtr processed_cloud_;
    int processed_point_count_{0};
    rclcpp::Time start_time_;

    void processCloud();
};

4. 콜백 메서드 구현

4.1 onStart() 콜백

BT::NodeStatus ProcessPointCloudAction::onStart()
{
    std::string lidar_topic;
    getInput("lidar_topic", lidar_topic);

    data_received_ = false;
    processing_done_ = false;
    raw_cloud_.reset();

    subscription_ =
        node_->create_subscription<
            sensor_msgs::msg::PointCloud2>(
            lidar_topic,
            rclcpp::SensorDataQoS(),
            [this](const%20sensor_msgs::msg::PointCloud2::SharedPtr%20msg)
            {
                if (!data_received_)
                {
                    raw_cloud_ = msg;
                    data_received_ = true;
                }
            });

    start_time_ = node_->now();
    return BT::NodeStatus::RUNNING;
}

4.2 onRunning() 콜백

BT::NodeStatus ProcessPointCloudAction::onRunning()
{
    double timeout;
    getInput("timeout", timeout);
    auto elapsed = (node_->now() - start_time_).seconds();

    if (elapsed > timeout)
    {
        RCLCPP_WARN(node_->get_logger(),
                    "포인트 클라우드 수신 타임아웃");
        subscription_.reset();
        return BT::NodeStatus::FAILURE;
    }

    if (data_received_ && !processing_done_)
    {
        subscription_.reset();
        processCloud();
        processing_done_ = true;
    }

    if (processing_done_)
    {
        setOutput("processed_cloud", processed_cloud_);
        setOutput("point_count", processed_point_count_);

        RCLCPP_DEBUG(node_->get_logger(),
            "포인트 클라우드 처리 완료: %d점",
            processed_point_count_);

        return BT::NodeStatus::SUCCESS;
    }

    return BT::NodeStatus::RUNNING;
}

4.3 onHalted() 콜백

void ProcessPointCloudAction::onHalted()
{
    subscription_.reset();
    data_received_ = false;
    processing_done_ = false;
    raw_cloud_.reset();
}

5. 포인트 클라우드 처리 파이프라인

5.1 processCloud() 구현

포인트 클라우드 처리는 다음 단계를 순차적으로 수행한다.

void ProcessPointCloudAction::processCloud()
{
    // 1. ROS2 메시지를 PCL로 변환
    CloudT::Ptr cloud(new CloudT);
    pcl::fromROSMsg(*raw_cloud_, *cloud);

    RCLCPP_DEBUG(node_->get_logger(),
                 "원본 포인트 수: %zu", cloud->size());

    // 2. 거리 필터링
    double range_min, range_max;
    getInput("range_min", range_min);
    getInput("range_max", range_max);
    cloud = applyRangeFilter(cloud, range_min, range_max);

    // 3. 복셀 다운샘플링
    double voxel_size;
    getInput("voxel_size", voxel_size);
    cloud = applyVoxelDownsampling(cloud, voxel_size);

    // 4. 지면 제거 (선택적)
    bool remove_ground;
    getInput("remove_ground", remove_ground);
    if (remove_ground)
    {
        cloud = removeGroundPlane(cloud);
    }

    // 5. 결과를 ROS2 메시지로 변환
    processed_cloud_ =
        std::make_shared<sensor_msgs::msg::PointCloud2>();
    pcl::toROSMsg(*cloud, *processed_cloud_);
    processed_cloud_->header = raw_cloud_->header;
    processed_point_count_ =
        static_cast<int>(cloud->size());
}

5.2 거리 필터링

센서 근접 영역의 잡음과 최대 측정 범위를 초과하는 무효 측정값을 제거한다.

CloudT::Ptr applyRangeFilter(
    const CloudT::Ptr& input,
    double range_min,
    double range_max)
{
    CloudT::Ptr filtered(new CloudT);
    for (const auto& pt : input->points)
    {
        double range = std::sqrt(
            pt.x * pt.x + pt.y * pt.y + pt.z * pt.z);
        if (range >= range_min && range <= range_max &&
            std::isfinite(pt.x) &&
            std::isfinite(pt.y) &&
            std::isfinite(pt.z))
        {
            filtered->points.push_back(pt);
        }
    }
    filtered->width = filtered->points.size();
    filtered->height = 1;
    filtered->is_dense = true;
    return filtered;
}

std::isfinite() 검사를 포함하여 NaN 및 무한대 값을 가진 무효 측정점을 제거한다. 라이다 센서는 반사 표면이 없는 방향에서 NaN 값을 반환할 수 있다.

5.3 복셀 다운샘플링

복셀 그리드 필터(voxel grid filter)는 3차원 공간을 균등한 복셀(voxel)로 분할하고, 각 복셀 내의 점들을 대표점(중심점)으로 대체하여 데이터를 축소한다.

#include <pcl/filters/voxel_grid.h>

CloudT::Ptr applyVoxelDownsampling(
    const CloudT::Ptr& input,
    double voxel_size)
{
    CloudT::Ptr downsampled(new CloudT);
    pcl::VoxelGrid<PointT> voxel_filter;
    voxel_filter.setInputCloud(input);
    voxel_filter.setLeafSize(
        voxel_size, voxel_size, voxel_size);
    voxel_filter.filter(*downsampled);
    return downsampled;
}

복셀 크기는 처리 정밀도와 연산 시간 사이의 트레이드오프를 결정한다. 복셀 크기가 작을수록 공간 해상도가 높아지지만 연산 비용이 증가하고, 클수록 처리 속도가 향상되지만 세밀한 구조 정보가 손실된다.

5.4 지면 제거

RANSAC(Random Sample Consensus) 알고리즘을 이용한 평면 추정으로 지면을 제거한다.

#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>

CloudT::Ptr removeGroundPlane(const CloudT::Ptr& input)
{
    // RANSAC 기반 평면 추정
    pcl::SACSegmentation<PointT> seg;
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setDistanceThreshold(0.15);
    seg.setInputCloud(input);

    pcl::ModelCoefficients::Ptr coefficients(
        new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(
        new pcl::PointIndices);
    seg.segment(*inliers, *coefficients);

    if (inliers->indices.empty())
    {
        return input;
    }

    // 지면 점 제거
    CloudT::Ptr non_ground(new CloudT);
    pcl::ExtractIndices<PointT> extract;
    extract.setInputCloud(input);
    extract.setIndices(inliers);
    extract.setNegative(true);  // 인라이어 제외
    extract.filter(*non_ground);

    return non_ground;
}

setDistanceThreshold(0.15)는 추정된 평면으로부터 0.15미터 이내의 점을 지면으로 판정한다. 이 값은 지면의 거칠기(roughness)와 센서의 측정 잡음에 따라 조정해야 한다.

6. 좌표 변환

포인트 클라우드 데이터를 로봇의 기준 좌표계(base_link)로 변환하기 위해 TF2를 활용한다.

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

// TF2를 이용한 좌표 변환
geometry_msgs::msg::TransformStamped transform;
try
{
    transform = tf_buffer_->lookupTransform(
        "base_link",
        cloud_msg->header.frame_id,
        cloud_msg->header.stamp,
        rclcpp::Duration::from_seconds(0.1));

    sensor_msgs::msg::PointCloud2 transformed_cloud;
    tf2::doTransform(*cloud_msg, transformed_cloud,
                     transform);
}
catch (const tf2::TransformException& ex)
{
    RCLCPP_WARN(node_->get_logger(),
                "좌표 변환 실패: %s", ex.what());
}

7. XML 행동 트리에서의 활용

7.1 노드 등록

factory.registerBuilder<ProcessPointCloudAction>(
    "ProcessPointCloud",
    [ros_node](const%20std::string&%20name,
%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20const%20BT::NodeConfiguration&%20config)
    {
        return std::make_unique<ProcessPointCloudAction>(
            name, config, ros_node);
    });

7.2 장애물 탐지 파이프라인

<Sequence>
    <ProcessPointCloud lidar_topic="/lidar/points"
                       voxel_size="0.05"
                       range_min="0.3"
                       range_max="50.0"
                       remove_ground="true"
                       processed_cloud="{obstacle_cloud}"
                       point_count="{num_points}" />
    <DetectObject cloud="{obstacle_cloud}"
                  detections="{obstacles}" />
</Sequence>

7.3 파라미터 조정을 통한 용도별 처리

정밀 매핑 용도에서는 복셀 크기를 작게, 실시간 장애물 탐지에서는 크게 설정한다.

<!-- 정밀 매핑 -->
<ProcessPointCloud voxel_size="0.02"
                   range_max="30.0"
                   remove_ground="false"
                   processed_cloud="{mapping_cloud}" />

<!-- 실시간 장애물 탐지 -->
<ProcessPointCloud voxel_size="0.2"
                   range_max="20.0"
                   remove_ground="true"
                   processed_cloud="{obstacle_cloud}" />

8. 참고 문헌

• Colledanchise, M. and Ögren, P., “Behavior Trees in Robotics and AI: An Introduction,” CRC Press, 2018.
• Rusu, R. B. and Cousins, S., “3D is here: Point Cloud Library (PCL),” IEEE International Conference on Robotics and Automation (ICRA), 2011.
• Fischler, M. A. and Bolles, R. C., “Random Sample Consensus: A Paradigm for Model Fitting with Applications to Image Analysis and Automated Cartography,” Communications of the ACM, vol. 24, no. 6, pp. 381-395, 1981.
• Faconti, D. and Contributors, “BehaviorTree.CPP: A C++ library to build Behavior Trees,” GitHub Repository, https://github.com/BehaviorTree/BehaviorTree.CPP.

버전: 2026-04-04