토픽 메시지 값 기반 조건 노드 (Topic Message Value-Based Condition Node)

토픽 메시지 값 기반 조건 노드 (Topic Message Value-Based Condition Node)

1. 개요

토픽 메시지 값 기반 조건 노드는 ROS2 토픽으로부터 수신된 메시지의 특정 값을 평가하여 조건의 참/거짓을 판정하는 조건 노드이다. 단순히 메시지의 수신 여부만을 확인하는 것에서 나아가, 메시지에 포함된 수치, 문자열, 불리언 등의 데이터를 임계값과 비교하거나 특정 조건식에 대입하여 행동 트리의 제어 흐름을 결정한다. 이 노드는 센서 측정값 평가, 상태 변수 확인, 시스템 매개변수 검증 등 로봇 시스템의 의사 결정에 폭넓게 적용된다.

2. 설계 원리

2.1 값 기반 조건 평가의 구조

토픽 메시지 값 기반 조건 노드의 동작은 다음 세 단계로 구성된다.

  1. 메시지 수신: ROS2 구독자를 통해 토픽 메시지를 수신하고 내부 버퍼에 저장한다.
  2. 값 추출: 수신된 메시지로부터 조건 평가에 필요한 특정 값을 추출한다.
  3. 조건 평가: 추출된 값을 입력 포트로 전달받은 기준값(threshold)이나 조건식과 비교하여 SUCCESS 또는 FAILURE를 반환한다.

이 구조에서 핵심은 2단계의 값 추출 과정이다. ROS2 메시지는 다양한 필드를 포함하는 복합 구조체이므로, 조건 평가에 필요한 특정 필드를 정확히 지정하여 추출하여야 한다.

2.2 비교 연산의 유형

메시지 값에 대한 조건 평가는 다음과 같은 비교 연산을 포함한다.

비교 유형수학적 표현설명
등호 비교v = c값이 기준값과 동일한지 판정
부등호 비교 (초과)v > c값이 기준값을 초과하는지 판정
부등호 비교 (미만)v < c값이 기준값 미만인지 판정
범위 비교c_{\min} \leq v \leq c_{\max}값이 지정된 범위 내에 있는지 판정
부등 비교v \neq c값이 기준값과 다른지 판정

여기서 v는 메시지로부터 추출된 값이며, c, c_{\min}, c_{\max}는 블랙보드 입력 포트를 통해 전달받는 기준값이다.

3. 기본 구현

3.1 임계값 비교 조건 노드

가장 일반적인 형태는 메시지의 특정 값을 단일 임계값과 비교하는 조건 노드이다. 다음은 std_msgs::msg::Float64 메시지의 data 필드를 임계값과 비교하는 구현이다.

class IsValueAboveThreshold
    : public BT::RosTopicSubNode<std_msgs::msg::Float64>
{
public:
    IsValueAboveThreshold(const std::string& name,
                          const BT::NodeConfiguration& config,
                          const BT::RosNodeParams& params)
        : RosTopicSubNode(name, config, params)
    {}

    static BT::PortsList providedPorts()
    {
        return providedBasicPorts({
            BT::InputPort<double>("threshold", "비교 임계값")
        });
    }

    BT::NodeStatus onTick(
        const std_msgs::msg::Float64::SharedPtr& msg) override
    {
        if (!msg)
        {
            return BT::NodeStatus::FAILURE;
        }

        double threshold;
        getInput("threshold", threshold);

        if (msg->data > threshold)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

이 구현에서 threshold 입력 포트의 값은 XML 행동 트리 정의에서 상수로 지정하거나, 블랙보드 키를 통해 동적으로 전달할 수 있다.

3.2 범위 비교 조건 노드

값이 지정된 범위 [c_{\min}, c_{\max}] 내에 있는지를 판정하는 조건 노드이다.

class IsValueInRange
    : public BT::RosTopicSubNode<std_msgs::msg::Float64>
{
public:
    IsValueInRange(const std::string& name,
                   const BT::NodeConfiguration& config,
                   const BT::RosNodeParams& params)
        : RosTopicSubNode(name, config, params)
    {}

    static BT::PortsList providedPorts()
    {
        return providedBasicPorts({
            BT::InputPort<double>("min_value", "하한값"),
            BT::InputPort<double>("max_value", "상한값")
        });
    }

    BT::NodeStatus onTick(
        const std_msgs::msg::Float64::SharedPtr& msg) override
    {
        if (!msg)
        {
            return BT::NodeStatus::FAILURE;
        }

        double min_val, max_val;
        getInput("min_value", min_val);
        getInput("max_value", max_val);

        if (msg->data >= min_val && msg->data <= max_val)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

4. 복합 메시지 타입에 대한 값 평가

4.1 Odometry 메시지 기반 속도 조건

실제 로봇 시스템에서는 단순 스칼라 메시지보다 복합 메시지 타입을 다루는 경우가 대부분이다. 다음은 nav_msgs::msg::Odometry 메시지에서 선속도(linear velocity)를 추출하여 임계값과 비교하는 예시이다.

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

    static BT::PortsList providedPorts()
    {
        return providedBasicPorts({
            BT::InputPort<double>("max_speed", 1.0, "최대 허용 선속도 (m/s)")
        });
    }

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

        double max_speed;
        getInput("max_speed", max_speed);

        double vx = msg->twist.twist.linear.x;
        double vy = msg->twist.twist.linear.y;
        double speed = std::sqrt(vx * vx + vy * vy);

        if (speed < max_speed)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

선속도의 크기(magnitude)는 \lVert \mathbf{v} \rVert = \sqrt{v_x^2 + v_y^2}로 계산하며, 이를 max_speed 임계값과 비교한다. 이와 같이 복합 메시지로부터 의미 있는 값을 도출하기 위해 중간 계산이 필요한 경우가 있으나, 이 계산은 순수 함수(pure function)의 성격을 가지므로 조건 노드의 부작용 금지 원칙에 위배되지 않는다.

4.2 LaserScan 메시지 기반 최소 거리 조건

sensor_msgs::msg::LaserScan 메시지에서 최소 측정 거리를 추출하여 임계값과 비교하는 조건 노드이다.

class IsMinRangeAbove
    : public BT::RosTopicSubNode<sensor_msgs::msg::LaserScan>
{
public:
    IsMinRangeAbove(const std::string& name,
                    const BT::NodeConfiguration& config,
                    const BT::RosNodeParams& params)
        : RosTopicSubNode(name, config, params)
    {}

    static BT::PortsList providedPorts()
    {
        return providedBasicPorts({
            BT::InputPort<double>("min_distance", 0.3,
                                  "최소 허용 거리 (m)")
        });
    }

    BT::NodeStatus onTick(
        const sensor_msgs::msg::LaserScan::SharedPtr& msg) override
    {
        if (!msg)
        {
            return BT::NodeStatus::FAILURE;
        }

        double min_distance;
        getInput("min_distance", min_distance);

        for (size_t i = 0; i < msg->ranges.size(); ++i)
        {
            float range = msg->ranges[i];
            if (std::isfinite(range) &&
                range >= msg->range_min &&
                range <= msg->range_max &&
                range < min_distance)
            {
                return BT::NodeStatus::FAILURE;
            }
        }
        return BT::NodeStatus::SUCCESS;
    }
};

LaserScan 메시지의 ranges 배열에는 NaN, Inf 등 무효한 값이 포함될 수 있으므로, std::isfinite() 검사와 함께 range_min, range_max 범위 내의 유효한 측정값만을 대상으로 평가하여야 한다.

5. 일반화된 값 기반 조건 노드 설계

5.1 비교 연산자 매개변수화

비교 연산의 유형을 입력 포트를 통해 지정함으로써, 단일 클래스로 다양한 비교 조건을 처리할 수 있다.

enum class ComparisonOp { EQ, NEQ, GT, LT, GTE, LTE };

class GenericValueCondition
    : public BT::RosTopicSubNode<std_msgs::msg::Float64>
{
public:
    GenericValueCondition(const std::string& name,
                          const BT::NodeConfiguration& config,
                          const BT::RosNodeParams& params)
        : RosTopicSubNode(name, config, params)
    {}

    static BT::PortsList providedPorts()
    {
        return providedBasicPorts({
            BT::InputPort<double>("reference", "기준값"),
            BT::InputPort<std::string>("operator", "EQ",
                "비교 연산자 (EQ, NEQ, GT, LT, GTE, LTE)")
        });
    }

    BT::NodeStatus onTick(
        const std_msgs::msg::Float64::SharedPtr& msg) override
    {
        if (!msg)
        {
            return BT::NodeStatus::FAILURE;
        }

        double reference;
        std::string op_str;
        getInput("reference", reference);
        getInput("operator", op_str);

        bool result = evaluate(msg->data, reference, op_str);
        return result ? BT::NodeStatus::SUCCESS
                      : BT::NodeStatus::FAILURE;
    }

private:
    bool evaluate(double value, double reference,
                  const std::string& op) const
    {
        if (op == "EQ")  return std::abs(value - reference) < 1e-9;
        if (op == "NEQ") return std::abs(value - reference) >= 1e-9;
        if (op == "GT")  return value > reference;
        if (op == "LT")  return value < reference;
        if (op == "GTE") return value >= reference;
        if (op == "LTE") return value <= reference;
        return false;
    }
};

등호 비교(EQ)에서는 부동소수점(floating-point) 수의 비교 특성을 고려하여 절대 오차 허용치(\epsilon = 10^{-9})를 적용한다. 부동소수점 연산의 정밀도 한계로 인해 직접적인 등호 비교는 예상치 못한 결과를 초래할 수 있기 때문이다.

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

6.1 속도 제한 조건을 활용한 안전 주행

<BehaviorTree ID="SafeSpeedNavigation">
    <ReactiveSequence>
        <Condition ID="IsLinearSpeedBelow"
                   topic_name="/odom"
                   max_speed="2.0"/>
        <Action ID="NavigateToWaypoint"/>
    </ReactiveSequence>
</BehaviorTree>

6.2 블랙보드를 통한 동적 임계값 지정

임계값을 상수가 아닌 블랙보드 키를 통해 동적으로 설정할 수 있다. 이를 통해 운용 상황에 따라 조건의 기준값을 변경하는 것이 가능하다.

<BehaviorTree ID="AdaptiveCondition">
    <Sequence>
        <Action ID="ComputeThreshold"
                output_threshold="{dynamic_threshold}"/>
        <ReactiveSequence>
            <Condition ID="IsValueAboveThreshold"
                       topic_name="/sensor/data"
                       threshold="{dynamic_threshold}"/>
            <Action ID="ProcessData"/>
        </ReactiveSequence>
    </Sequence>
</BehaviorTree>

{dynamic_threshold} 구문은 블랙보드 키를 참조하며, ComputeThreshold 액션 노드가 계산한 값이 조건 노드의 임계값으로 전달된다.

7. 설계 시 고려 사항

7.1 수치 정밀도

부동소수점 값의 비교에서는 머신 엡실론(machine epsilon)에 의한 오차를 고려하여야 한다. 특히 등호 비교나 경계값 비교에서 미세한 수치 차이로 인해 조건 평가 결과가 불안정하게 진동(oscillation)하는 현상이 발생할 수 있다. 이를 방지하기 위해 히스테리시스(hysteresis) 기법을 적용하거나, 적절한 오차 허용치를 설정하는 것이 바람직하다.

7.2 메시지 값의 유효성 검증

센서 메시지에는 무효한 값(NaN, Inf, 음수 거리 등)이 포함될 수 있다. 조건 평가 전에 메시지 값의 유효성을 검증하는 로직을 포함하여야 하며, 무효한 값이 감지된 경우 FAILURE를 반환하는 것이 안전하다. 이는 잘못된 센서 데이터에 기반한 오판을 방지한다.

7.3 단위와 좌표계 일관성

메시지에 포함된 값과 비교 기준값의 단위 및 좌표계가 일치하여야 한다. 예를 들어, 속도가 m/s 단위로 발행되는 토픽에 대해 km/h 단위의 임계값을 적용하면 오류가 발생한다. ROS2에서는 SI 단위계와 REP-103에 정의된 좌표 규약을 따르는 것이 표준이다.

8. 참고 문헌

  • 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.
  • ROS Enhancement Proposal (REP) 103. Standard Units of Measure and Coordinate Conventions.
  • BehaviorTree.CPP 공식 문서. https://www.behaviortree.dev/
버전날짜변경 사항
v0.12026-04-04초안 작성