1296.74 DetectObject 액션 노드 구현

1. 개요

DetectObject 액션 노드는 카메라 이미지 또는 3차원 포인트 클라우드로부터 관심 물체를 탐지하고, 탐지 결과를 행동 트리의 블랙보드에 기록하는 리프 노드이다. 물체 탐지는 로봇이 환경 내의 대상을 인식하고 그에 따른 행동을 결정하는 핵심 인식 단계로서, 자율 주행, 물류 자동화, 검사 자동화 등 다양한 응용에서 활용된다.

물체 탐지 알고리즘은 심층 신경망(deep neural network) 기반의 추론을 수행하므로, 연산 시간이 수십 밀리초에서 수 초까지 소요될 수 있다. 따라서 BT::StatefulActionNode를 상속하여 비동기 방식으로 구현하며, 실제 추론은 별도의 ROS2 서비스 또는 액션 서버에 위임한다.

2. 물체 탐지 결과의 표현

2.1 Detection 메시지 정의

물체 탐지 결과는 탐지된 물체의 클래스, 신뢰도, 경계 상자(bounding box) 또는 3차원 위치 정보를 포함한다. ROS2에서는 vision_msgs 패키지의 표준 메시지 타입을 활용한다.

vision_msgs/msg/Detection2D는 2차원 이미지 상의 탐지 결과를 표현하며, 다음 필드를 포함한다.

vision_msgs/msg/Detection3D는 3차원 공간의 탐지 결과를 표현하며, 3차원 경계 상자와 자세(pose) 정보를 포함한다.

2.2 탐지 결과 배열

복수의 물체가 탐지되는 경우, vision_msgs/msg/Detection2DArray 또는 Detection3DArray를 사용하여 결과를 배열로 전달한다.

3. ROS2 서비스 인터페이스 설계

물체 탐지를 별도의 추론 서버에 위임하기 위한 서비스 인터페이스를 정의한다.

# DetectObjects.srv
# Request
sensor_msgs/Image image           # 입력 이미지
float64 confidence_threshold       # 최소 신뢰도 임계값
string[] target_classes            # 탐지 대상 클래스 (빈 배열: 전체)
---
# Response
vision_msgs/Detection2DArray detections  # 탐지 결과
bool success                             # 처리 성공 여부
string error_message                     # 오류 메시지

target_classes 필드를 통해 특정 클래스의 물체만 선택적으로 탐지할 수 있다. 예를 들어, 물류 환경에서는 ["box", "pallet"]만 지정하여 불필요한 탐지 결과를 제거한다.

4. 클래스 구조 설계

#include <behaviortree_cpp/action_node.h>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <vision_msgs/msg/detection2_d_array.hpp>
#include "perception_interfaces/srv/detect_objects.hpp"

class DetectObjectAction : public BT::StatefulActionNode
{
public:
    using DetectObjects =
        perception_interfaces::srv::DetectObjects;

    DetectObjectAction(
        const std::string& name,
        const BT::NodeConfiguration& config,
        rclcpp::Node::SharedPtr node)
        : BT::StatefulActionNode(name, config),
          node_(node)
    {
        service_client_ =
            node_->create_client<DetectObjects>(
                "detect_objects");
    }

    static BT::PortsList providedPorts()
    {
        return {
            BT::InputPort<std::shared_ptr<
                sensor_msgs::msg::Image>>("image",
                "입력 이미지"),
            BT::InputPort<double>("confidence_threshold",
                0.5, "최소 신뢰도 임계값"),
            BT::InputPort<std::string>("target_class", "",
                "탐지 대상 클래스 (빈 문자열: 전체)"),
            BT::InputPort<double>("timeout", 10.0,
                "탐지 타임아웃 (초)"),
            BT::OutputPort<std::shared_ptr<
                vision_msgs::msg::Detection2DArray>>(
                "detections", "탐지 결과"),
            BT::OutputPort<int>("detection_count",
                "탐지된 물체 수")
        };
    }

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

private:
    rclcpp::Node::SharedPtr node_;
    rclcpp::Client<DetectObjects>::SharedPtr service_client_;
    rclcpp::Client<DetectObjects>::SharedFuture future_;
    std::atomic<bool> request_sent_{false};
    rclcpp::Time start_time_;
};

5. 콜백 메서드 구현

5.1 onStart() 콜백

BT::NodeStatus DetectObjectAction::onStart()
{
    if (!service_client_->wait_for_service(
            std::chrono::seconds(5)))
    {
        RCLCPP_ERROR(node_->get_logger(),
                     "물체 탐지 서비스 연결 실패");
        return BT::NodeStatus::FAILURE;
    }

    // 입력 이미지 획득
    std::shared_ptr<sensor_msgs::msg::Image> image;
    if (!getInput("image", image) || !image)
    {
        RCLCPP_ERROR(node_->get_logger(),
                     "입력 이미지가 없음");
        return BT::NodeStatus::FAILURE;
    }

    double confidence_threshold;
    std::string target_class;
    getInput("confidence_threshold", confidence_threshold);
    getInput("target_class", target_class);

    // 서비스 요청 구성
    auto request =
        std::make_shared<DetectObjects::Request>();
    request->image = *image;
    request->confidence_threshold = confidence_threshold;
    if (!target_class.empty())
    {
        request->target_classes.push_back(target_class);
    }

    // 비동기 서비스 호출
    future_ = service_client_->async_send_request(request);
    request_sent_ = true;
    start_time_ = node_->now();

    return BT::NodeStatus::RUNNING;
}

5.2 onRunning() 콜백

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

    if (elapsed > timeout)
    {
        RCLCPP_WARN(node_->get_logger(),
                    "물체 탐지 타임아웃 (%.1f초)", elapsed);
        return BT::NodeStatus::FAILURE;
    }

    // 서비스 응답 확인
    if (future_.valid() &&
        future_.wait_for(std::chrono::milliseconds(0)) ==
            std::future_status::ready)
    {
        auto response = future_.get();

        if (!response->success)
        {
            RCLCPP_ERROR(node_->get_logger(),
                "탐지 실패: %s",
                response->error_message.c_str());
            return BT::NodeStatus::FAILURE;
        }

        auto detections =
            std::make_shared<
                vision_msgs::msg::Detection2DArray>(
                    response->detections);
        int count =
            static_cast<int>(detections->detections.size());

        setOutput("detections", detections);
        setOutput("detection_count", count);

        RCLCPP_INFO(node_->get_logger(),
                    "물체 탐지 완료: %d개 탐지", count);

        return BT::NodeStatus::SUCCESS;
    }

    return BT::NodeStatus::RUNNING;
}

물체가 0개 탐지된 경우에도 SUCCESS를 반환한다. 이는 탐지 알고리즘이 정상적으로 실행되었으나 관심 물체가 존재하지 않는 환경 조건을 의미하기 때문이다. 탐지된 물체의 존재 여부에 따른 행동 분기는 후속 조건 노드에서 판단하는 것이 단일 책임 원칙에 부합한다.

5.3 onHalted() 콜백

void DetectObjectAction::onHalted()
{
    request_sent_ = false;
}

6. 탐지 알고리즘 연동

6.1 차원 물체 탐지

2차원 이미지 기반 물체 탐지에는 YOLO(You Only Look Once), SSD(Single Shot MultiBox Detector), Faster R-CNN 등의 심층 신경망 모델이 활용된다. 이러한 모델은 ROS2 서비스 서버 내에서 추론 프레임워크(TensorRT, ONNX Runtime, PyTorch 등)를 통해 실행된다.

추론 서버의 구현 예시는 다음과 같다.

# ROS2 물체 탐지 서비스 서버 (Python)
import rclpy
from rclpy.node import Node
from perception_interfaces.srv import DetectObjects
from ultralytics import YOLO

class DetectionServer(Node):
    def __init__(self):
        super().__init__('detection_server')
        self.model = YOLO('yolov8n.pt')
        self.srv = self.create_service(
            DetectObjects,
            'detect_objects',
            self.detect_callback)

    def detect_callback(self, request, response):
        # 이미지 변환 및 추론 수행
        results = self.model(cv_image,
                            conf=request.confidence_threshold)
        # 결과를 Detection2DArray로 변환
        response.success = True
        return response

6.2 차원 물체 탐지

포인트 클라우드 기반 3차원 물체 탐지에는 PointPillars, VoxelNet, CenterPoint 등의 모델이 활용된다. 3차원 탐지 결과는 vision_msgs/msg/Detection3DArray로 표현하며, 각 탐지 결과는 3차원 경계 상자(oriented bounding box)와 자세 정보를 포함한다.

6.3 다중 센서 융합 탐지

카메라 이미지와 포인트 클라우드를 동시에 입력으로 받아 탐지 성능을 향상시키는 융합(fusion) 방식도 존재한다. 이 경우 DetectObject 노드는 이미지와 포인트 클라우드를 모두 입력 포트로 수신하도록 설계한다.

7. 탐지 결과 후처리

7.1 비최대 억제 (Non-Maximum Suppression)

동일 물체에 대한 중복 탐지를 제거하기 위해 비최대 억제(NMS) 알고리즘을 적용한다. IoU(Intersection over Union) 임계값을 기준으로 중첩 정도가 높은 탐지 결과 중 신뢰도가 가장 높은 것만 유지한다.

\text{IoU}(A, B) = \frac{\lvert A \cap B \rvert}{\lvert A \cup B \rvert}

여기서 A와 B는 두 경계 상자의 영역이다. 일반적으로 IoU 임계값은 0.45~0.5로 설정한다.

7.2 좌표 변환

2차원 탐지 결과를 로봇의 3차원 좌표계로 투영하려면 카메라의 내부 파라미터(intrinsic parameters)와 깊이 정보가 필요하다. 핀홀(pinhole) 카메라 모델에서 이미지 좌표 (u, v)와 3차원 좌표 (X, Y, Z)의 관계는 다음과 같다.

\begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \frac{1}{Z} \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X \\ Y \\ Z \end{bmatrix}

여기서 f_x, f_y는 초점 거리, c_x, c_y는 주점(principal point) 좌표이다.

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

8.1 노드 등록

factory.registerBuilder<DetectObjectAction>(
    "DetectObject",
    [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<DetectObjectAction>(
            name, config, ros_node);
    });

8.2 인식 기반 조건 분기

탐지된 물체의 수에 따라 행동을 분기하는 구조이다.

<Sequence>
    <CaptureImage camera_topic="/camera/image_raw"
                  image="{image}" />
    <DetectObject image="{image}"
                  confidence_threshold="0.6"
                  target_class="person"
                  detections="{detections}"
                  detection_count="{num_persons}" />
    <Fallback>
        <Sequence>
            <CheckGreaterThan value="{num_persons}"
                              threshold="0" />
            <ApproachTarget detections="{detections}" />
        </Sequence>
        <ContinuePatrol />
    </Fallback>
</Sequence>

이 구조에서 사람이 탐지되면 접근 동작을 수행하고, 탐지되지 않으면 순찰을 계속한다.

8.3 반복 탐지와 추적

지속적인 물체 탐지를 수행하며 추적하는 구조이다.

<ReactiveSequence>
    <CaptureImage camera_topic="/camera/image_raw"
                  image="{image}" />
    <DetectObject image="{image}"
                  confidence_threshold="0.5"
                  target_class="target"
                  detections="{detections}" />
    <TrackObject detections="{detections}" />
</ReactiveSequence>

ReactiveSequence는 매 tick마다 첫 번째 자식부터 재평가하므로, 이미지 획득과 물체 탐지가 반복적으로 수행되어 실시간 추적이 가능하다.

9. 성능 고려 사항

물체 탐지 모델의 추론 시간은 모델 크기, 입력 해상도, 실행 하드웨어에 따라 크게 달라진다.

실시간 응용에서는 경량 모델(YOLOv8n 등)을 선택하고, 정밀도가 요구되는 응용에서는 대형 모델을 사용하되 GPU 가속을 적용해야 한다. NVIDIA Jetson 계열 임베디드 플랫폼에서는 TensorRT를 통한 모델 최적화가 추론 시간 단축에 효과적이다.

10. 참고 문헌

• Colledanchise, M. and Ögren, P., “Behavior Trees in Robotics and AI: An Introduction,” CRC Press, 2018.
• Redmon, J. et al., “You Only Look Once: Unified, Real-Time Object Detection,” IEEE Conference on Computer Vision and Pattern Recognition (CVPR), 2016.
• Ren, S. et al., “Faster R-CNN: Towards Real-Time Object Detection with Region Proposal Networks,” Advances in Neural Information Processing Systems (NeurIPS), 2015.
• Lang, A. H. et al., “PointPillars: Fast Encoders for Object Detection from Point Clouds,” IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR), 2019.
• Faconti, D. and Contributors, “BehaviorTree.CPP: A C++ library to build Behavior Trees,” GitHub Repository, https://github.com/BehaviorTree/BehaviorTree.CPP.

버전: 2026-04-04