토픽 메시지 수신 여부 확인 조건 노드 (Topic Message Reception Check Condition Node)

1. 개요

토픽 메시지 수신 여부 확인 조건 노드는 ROS2 토픽 기반 조건 노드의 가장 기본적인 형태로서, 특정 토픽으로부터 메시지가 수신되었는지를 판정한다. 이 조건 노드는 메시지의 내용이나 값을 분석하지 않으며, 오직 메시지가 도착하였는지 여부만을 평가하여 SUCCESS 또는 FAILURE를 반환한다. 센서 활성화 상태 확인, 통신 채널 가용성 검증, 시스템 초기화 완료 판정 등 다양한 상황에서 전제 조건(precondition)으로 활용된다.

2. 설계 원리

2.1 수신 여부 판정의 정의

토픽 메시지 수신 여부는 다음 두 가지 기준 중 하나 이상을 충족하는지에 따라 결정된다.

1. 단순 수신 확인: 노드 생성 이후 해당 토픽으로부터 최소 하나의 메시지를 수신하였는가.
2. 시간 제한 수신 확인: 현재 시점으로부터 지정된 시간 이내에 메시지를 수신하였는가.

단순 수신 확인은 시스템 초기화 단계에서 각 구성 요소가 정상적으로 가동되었는지를 확인하는 데 적합하다. 반면, 시간 제한 수신 확인은 운용 중 센서 장애나 통신 두절을 감지하는 데 유용하다.

2.2 반환 상태 규칙

조건 노드의 원칙에 따라 RUNNING 상태는 반환하지 않는다. 메시지가 아직 도착하지 않은 경우에도 대기하지 않고 즉시 FAILURE를 반환한다.

3. 기본 구현

3.1 단순 수신 확인 구현

가장 단순한 형태의 토픽 메시지 수신 여부 확인 조건 노드는 구독 콜백에서 플래그(flag)를 설정하고, tick() 메서드에서 해당 플래그를 확인하는 구조이다.

template <typename MessageT>
class IsTopicReceived : public BT::ConditionNode
{
public:
    IsTopicReceived(const std::string& name,
                    const BT::NodeConfiguration& config,
                    rclcpp::Node::SharedPtr node)
        : BT::ConditionNode(name, config),
          node_(node),
          message_received_(false)
    {
        std::string topic_name;
        getInput("topic_name", topic_name);

        subscription_ = node_->create_subscription<MessageT>(
            topic_name,
            rclcpp::QoS(1),
            [this](const%20typename%20MessageT::SharedPtr%20msg) {
                std::lock_guard<std::mutex> lock(mutex_);
                message_received_ = true;
                last_received_time_ = node_->get_clock()->now();
            });
    }

    static BT::PortsList providedPorts()
    {
        return {
            BT::InputPort<std::string>("topic_name", "구독할 토픽 이름")
        };
    }

    BT::NodeStatus tick() override
    {
        std::lock_guard<std::mutex> lock(mutex_);
        if (message_received_)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }

private:
    rclcpp::Node::SharedPtr node_;
    typename rclcpp::Subscription<MessageT>::SharedPtr subscription_;
    std::mutex mutex_;
    bool message_received_;
    rclcpp::Time last_received_time_;
};

위 구현에서 message_received_ 플래그는 한 번 true로 설정되면 이후 tick에서 항상 SUCCESS를 반환한다. 이는 “한 번이라도 수신되었는가“라는 의미론을 따르는 것이다.

3.2 시간 제한 수신 확인 구현

운용 중 센서 장애나 통신 두절을 감지하기 위해서는, 단순히 메시지가 수신된 적이 있는지가 아니라 최근에 수신되었는지를 확인하여야 한다.

template <typename MessageT>
class IsTopicReceivedRecently : public BT::ConditionNode
{
public:
    IsTopicReceivedRecently(const std::string& name,
                            const BT::NodeConfiguration& config,
                            rclcpp::Node::SharedPtr node)
        : BT::ConditionNode(name, config),
          node_(node),
          message_received_(false)
    {
        std::string topic_name;
        getInput("topic_name", topic_name);

        subscription_ = node_->create_subscription<MessageT>(
            topic_name,
            rclcpp::QoS(1),
            [this](const%20typename%20MessageT::SharedPtr%20msg) {
                std::lock_guard<std::mutex> lock(mutex_);
                message_received_ = true;
                last_received_time_ = node_->get_clock()->now();
            });
    }

    static BT::PortsList providedPorts()
    {
        return {
            BT::InputPort<std::string>("topic_name", "구독할 토픽 이름"),
            BT::InputPort<double>("timeout_sec", 1.0,
                                  "메시지 유효 시간 (초)")
        };
    }

    BT::NodeStatus tick() override
    {
        std::lock_guard<std::mutex> lock(mutex_);
        if (!message_received_)
        {
            return BT::NodeStatus::FAILURE;
        }

        double timeout_sec;
        getInput("timeout_sec", timeout_sec);

        auto elapsed = node_->get_clock()->now() - last_received_time_;
        if (elapsed.seconds() > timeout_sec)
        {
            return BT::NodeStatus::FAILURE;
        }
        return BT::NodeStatus::SUCCESS;
    }

private:
    rclcpp::Node::SharedPtr node_;
    typename rclcpp::Subscription<MessageT>::SharedPtr subscription_;
    std::mutex mutex_;
    bool message_received_;
    rclcpp::Time last_received_time_;
};

timeout_sec 매개변수는 블랙보드 입력 포트를 통해 외부에서 지정할 수 있으며, 기본값은 1.0초이다. 마지막 수신 시각으로부터 경과한 시간이 이 값을 초과하면 FAILURE를 반환한다.

4. RosTopicSubNode 기반 구현

BehaviorTree.ROS2 패키지가 제공하는 RosTopicSubNode<MessageT> 기반 클래스를 활용하면 구독 관리 코드를 직접 작성하지 않고도 동일한 기능을 구현할 수 있다.

class IsOdomReceived
    : public BT::RosTopicSubNode<nav_msgs::msg::Odometry>
{
public:
    IsOdomReceived(const std::string& name,
                   const BT::NodeConfiguration& config,
                   const BT::RosNodeParams& params)
        : RosTopicSubNode(name, config, params)
    {}

    static BT::PortsList providedPorts()
    {
        return providedBasicPorts({});
    }

    BT::NodeStatus onTick(
        const nav_msgs::msg::Odometry::SharedPtr& msg) override
    {
        if (msg)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

onTick() 메서드의 인자 msg는 가장 최근에 수신된 메시지를 가리킨다. 메시지가 아직 수신되지 않은 경우 nullptr이 전달되므로, 널 포인터(null pointer) 검사만으로 수신 여부를 판정할 수 있다.

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

5.1 시스템 초기화 전제 조건

시스템 초기화 단계에서 모든 필수 토픽이 활성화되었는지를 확인하는 데 활용할 수 있다.

<BehaviorTree ID="SystemInit">
    <Sequence>
        <Condition ID="IsOdomReceived"
                   topic_name="/odom"/>
        <Condition ID="IsLaserScanReceived"
                   topic_name="/scan"/>
        <Condition ID="IsCameraReceived"
                   topic_name="/camera/image_raw"/>
        <Action ID="StartNavigation"/>
    </Sequence>
</BehaviorTree>

위 구조에서 Sequence 노드는 세 개의 조건 노드가 모두 SUCCESS를 반환할 때에만 StartNavigation 액션을 실행한다. 이를 통해 센서가 준비되지 않은 상태에서 내비게이션이 시작되는 것을 방지한다.

5.2 센서 건전성 지속 감시

ReactiveSequence와 결합하면 운용 중 센서 데이터의 지속적인 수신 여부를 감시할 수 있다.

<BehaviorTree ID="SafeNavigation">
    <ReactiveSequence>
        <Condition ID="IsLidarReceivedRecently"
                   topic_name="/scan"
                   timeout_sec="0.5"/>
        <Action ID="NavigateToGoal"/>
    </ReactiveSequence>
</BehaviorTree>

ReactiveSequence는 매 tick마다 조건 노드를 재평가하므로, 라이다(LiDAR) 데이터가 0.5초 이상 수신되지 않으면 NavigateToGoal 액션이 즉시 중단된다. 이는 센서 장애 시 로봇이 맹목적으로 이동하는 것을 방지하는 안전 메커니즘으로 기능한다.

6. 다중 토픽 수신 확인 패턴

복수의 토픽에 대한 수신 여부를 동시에 확인하여야 하는 경우, 개별 조건 노드를 Sequence로 결합하는 방법과 단일 노드에서 복수 토픽을 관리하는 방법이 있다.

6.1 Sequence 기반 다중 토픽 확인

<Sequence name="AllSensorsReady">
    <Condition ID="IsTopicReceived" topic_name="/imu/data"/>
    <Condition ID="IsTopicReceived" topic_name="/gps/fix"/>
    <Condition ID="IsTopicReceived" topic_name="/odom"/>
</Sequence>

이 방법은 각 토픽에 대해 독립적인 조건 노드를 사용하므로, 어떤 토픽이 수신되지 않았는지를 개별적으로 파악할 수 있다는 장점이 있다. 또한 로깅이나 디버깅 시 각 조건의 평가 결과를 분리하여 확인할 수 있다.

7. 팩토리 등록과 인스턴스화

토픽 메시지 수신 여부 확인 조건 노드를 행동 트리 팩토리에 등록하는 과정은 다음과 같다.

BT::BehaviorTreeFactory factory;
BT::RosNodeParams params;
params.nh = ros_node;

// Odometry 수신 확인 등록
params.default_port_value = "/odom";
factory.registerNodeType<IsOdomReceived>("IsOdomReceived", params);

// LaserScan 수신 확인 등록
params.default_port_value = "/scan";
factory.registerNodeType<IsLaserScanReceived>(
    "IsLaserScanReceived", params);

default_port_value를 설정하면, XML에서 토픽 이름을 명시하지 않은 경우 기본 토픽을 구독한다. 그러나 명시적으로 topic_name 속성을 지정하면 기본값을 재정의(override)할 수 있다.

8. 설계 시 고려 사항

8.1 구독 생존 주기

조건 노드의 구독자는 노드가 생성될 때 함께 생성되며, 노드가 소멸될 때 함께 소멸된다. 행동 트리가 서브트리(subtree)를 동적으로 교체하는 경우, 교체 시점에 구독자가 재생성되므로 일시적으로 메시지 수신이 중단될 수 있다. 이 문제를 방지하기 위해, 구독자의 생존 주기를 행동 트리 노드와 분리하여 관리하는 방안을 고려할 수 있다.

8.2 QoS 호환성

토픽 발행자의 QoS 설정과 조건 노드 구독자의 QoS 설정이 호환되지 않으면 메시지가 수신되지 않는다. 특히 센서 드라이버가 BEST_EFFORT 신뢰성을 사용하는 경우, 구독자도 동일하게 BEST_EFFORT를 사용하여야 한다. RELIABLE 구독자는 BEST_EFFORT 발행자의 메시지를 수신할 수 없기 때문이다.

8.3 초기 메시지 지연

ROS2 DDS(Data Distribution Service) 구현에 따라, 구독자가 생성된 직후에는 발행자를 발견(discovery)하는 과정에서 수백 밀리초의 지연이 발생할 수 있다. 시스템 초기화 시 이 지연을 고려하여, 조건 노드가 즉시 FAILURE를 반환하더라도 제어 노드의 재시도 메커니즘을 통해 충분한 시간 후 재평가되도록 행동 트리를 설계하여야 한다.

9. 참고 문헌

• Colledanchise, M., & Ogren, P. (2018). Behavior Trees in Robotics and AI: An Introduction. CRC Press.
• Macenski, S., et al. (2020). “The Marathon 2: A Navigation System.” arXiv preprint arXiv:2003.00368.
• BehaviorTree.CPP 공식 문서. https://www.behaviortree.dev/
• ROS2 공식 문서. https://docs.ros.org/en/humble/