토픽 메시지 필드 비교 조건 노드 (Topic Message Field Comparison Condition Node)

1. 개요

토픽 메시지 필드 비교 조건 노드는 ROS2 토픽 메시지의 특정 필드를 기준값과 비교하여 조건을 판정하는 조건 노드이다. ROS2 메시지는 일반적으로 여러 계층의 중첩된 필드(nested field)를 포함하는 복합 구조체로 정의되며, 조건 평가에 필요한 정보는 이러한 중첩 구조의 특정 필드에 위치한다. 본 절에서는 메시지의 개별 필드를 지정하여 접근하고, 해당 필드의 값을 다양한 비교 연산으로 평가하는 조건 노드의 설계와 구현을 다룬다.

2. 필드 접근 방식

2.1 정적 필드 접근

정적 필드 접근(static field access)은 조건 노드의 구현 시점에서 비교 대상 필드가 확정되어 있는 방식이다. C++ 컴파일 타임에 메시지 구조체의 멤버 변수에 직접 접근하므로, 타입 안전성(type safety)이 보장되며 런타임 오버헤드가 발생하지 않는다.

class IsPositionXAbove
    : public BT::RosTopicSubNode<geometry_msgs::msg::PoseStamped>
{
public:
    IsPositionXAbove(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", "X 좌표 기준값")
        });
    }

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

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

        if (msg->pose.position.x > threshold)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

위 구현에서 msg->pose.position.x는 PoseStamped 메시지의 중첩 필드 pose.position.x에 직접 접근한다. 이 방식은 단일 필드에 대한 조건을 구현할 때 가장 명확하고 효율적이다.

2.2 중첩 필드 접근 패턴

ROS2 메시지의 필드 계층 구조는 메시지 타입에 따라 상이하다. 다음 표는 로봇 시스템에서 빈번히 사용되는 메시지 타입과 그 중첩 필드 경로를 정리한 것이다.

3. 단일 필드 비교 조건 노드 구현

3.1 수치 필드 비교

수치형 필드를 임계값과 비교하는 조건 노드의 일반적 패턴은 다음과 같다.

class IsAngularVelocityBelow
    : public BT::RosTopicSubNode<nav_msgs::msg::Odometry>
{
public:
    IsAngularVelocityBelow(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_angular_vel", 1.0,
                                  "최대 허용 각속도 (rad/s)")
        });
    }

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

        double max_angular_vel;
        getInput("max_angular_vel", max_angular_vel);

        double angular_z = std::abs(msg->twist.twist.angular.z);
        if (angular_z <= max_angular_vel)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

각속도의 부호는 회전 방향을 나타내므로, 크기를 비교할 때에는 std::abs()를 적용하여 절대값으로 변환하는 것이 적절하다.

3.2 열거형 필드 비교

ROS2 메시지의 일부 필드는 정수형 열거 상수(enumeration constant)로 정의된다. 이러한 필드를 비교할 때에는 메시지 정의에 명시된 상수를 사용하여야 한다.

class IsGpsFixed
    : public BT::RosTopicSubNode<sensor_msgs::msg::NavSatFix>
{
public:
    IsGpsFixed(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 sensor_msgs::msg::NavSatFix::SharedPtr& msg) override
    {
        if (!msg)
        {
            return BT::NodeStatus::FAILURE;
        }

        if (msg->status.status >=
            sensor_msgs::msg::NavSatStatus::STATUS_FIX)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

NavSatStatus 메시지는 다음과 같은 상태 상수를 정의한다.

status.status >= STATUS_FIX 조건을 사용하면, 기본 측위 이상의 모든 유효한 GPS 상태를 포괄적으로 판정할 수 있다.

4. 다중 필드 비교 조건 노드

4.1 동일 메시지 내 다중 필드 비교

단일 메시지의 여러 필드를 동시에 평가하여야 하는 경우가 있다. 예를 들어, 로봇의 위치가 특정 영역 내에 있는지를 판정하려면 X 좌표와 Y 좌표를 함께 평가하여야 한다.

class IsPositionInArea
    : public BT::RosTopicSubNode<geometry_msgs::msg::PoseStamped>
{
public:
    IsPositionInArea(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>("x_min", "X 좌표 하한"),
            BT::InputPort<double>("x_max", "X 좌표 상한"),
            BT::InputPort<double>("y_min", "Y 좌표 하한"),
            BT::InputPort<double>("y_max", "Y 좌표 상한")
        });
    }

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

        double x_min, x_max, y_min, y_max;
        getInput("x_min", x_min);
        getInput("x_max", x_max);
        getInput("y_min", y_min);
        getInput("y_max", y_max);

        double x = msg->pose.position.x;
        double y = msg->pose.position.y;

        if (x >= x_min && x <= x_max &&
            y >= y_min && y <= y_max)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

다중 필드 비교 시에는 단일 조건 평가 원칙과의 균형을 고려하여야 한다. “위치가 영역 내에 있는가“라는 단일 의미론적 질문에 대한 답을 구하기 위해 복수의 필드를 참조하는 것은 허용되나, 서로 관련 없는 조건을 하나의 노드에 결합하는 것은 지양하여야 한다.

4.2 필드 간 상호 비교

동일 메시지 내의 서로 다른 필드를 상호 비교하는 조건도 구현할 수 있다. 다음은 공분산 행렬의 대각 요소를 비교하여 위치 추정의 불확실성이 균일한지를 판정하는 예시이다.

class IsCovarianceBalanced
    : public BT::RosTopicSubNode<nav_msgs::msg::Odometry>
{
public:
    IsCovarianceBalanced(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_ratio", 10.0,
                                  "X/Y 공분산 비율 상한")
        });
    }

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

        double max_ratio;
        getInput("max_ratio", max_ratio);

        // pose.covariance는 6x6 행렬의 row-major 배열
        // [0]: x-x, [7]: y-y
        double cov_xx = msg->pose.covariance[0];
        double cov_yy = msg->pose.covariance[7];

        if (cov_xx <= 0.0 || cov_yy <= 0.0)
        {
            return BT::NodeStatus::FAILURE;
        }

        double ratio = (cov_xx > cov_yy) ? (cov_xx / cov_yy)
                                          : (cov_yy / cov_xx);
        if (ratio <= max_ratio)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

Odometry 메시지의 pose.covariance 필드는 6 \times 6 공분산 행렬을 행 우선(row-major) 형태의 1차원 배열로 저장한다. 인덱스 [0]은 \sigma_{xx}^2, 인덱스 [7]은 \sigma_{yy}^2에 해당한다.

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

<BehaviorTree ID="AreaMonitoring">
    <ReactiveSequence>
        <Condition ID="IsPositionInArea"
                   topic_name="/robot_pose"
                   x_min="-5.0" x_max="5.0"
                   y_min="-3.0" y_max="3.0"/>
        <Action ID="PerformTask"/>
    </ReactiveSequence>
</BehaviorTree>

블랙보드를 통해 영역 경계를 동적으로 지정하면, 운용 상황에 따라 작업 영역을 변경할 수 있다.

<Condition ID="IsPositionInArea"
           topic_name="/robot_pose"
           x_min="{area_x_min}" x_max="{area_x_max}"
           y_min="{area_y_min}" y_max="{area_y_max}"/>

6. 설계 시 고려 사항

6.1 필드 존재성 보장

ROS2 메시지의 모든 필드는 IDL(Interface Definition Language) 정의에 의해 컴파일 타임에 결정되므로, 메시지 타입이 올바르게 지정되었다면 필드의 존재성은 보장된다. 그러나 선택적(optional) 필드가 포함된 메시지의 경우, 해당 필드가 유효한 값으로 설정되었는지를 별도로 확인하여야 한다.

6.2 좌표 프레임 일관성

위치나 자세와 관련된 필드를 비교할 때, 메시지의 header.frame_id가 기대하는 좌표 프레임과 일치하는지를 확인하여야 한다. 예를 들어, map 프레임에서의 위치 조건을 평가할 때 odom 프레임의 메시지가 수신되면 잘못된 판정을 내릴 수 있다. 좌표 변환(transform)이 필요한 경우에는 tf2 라이브러리를 활용하여야 하나, 조건 노드 내부에서 좌표 변환을 수행하면 평가 시간이 증가하므로 별도의 액션 노드에서 변환을 처리하고 결과를 블랙보드에 기록하는 방식이 선호된다.

6.3 배열 필드 접근

LaserScan의 ranges, Imu의 orientation_covariance 등 배열 형태의 필드에 접근할 때에는 인덱스 범위를 검증하여야 한다. 배열의 크기는 메시지 발행 시점에 동적으로 결정되므로, 고정된 인덱스로 접근하기 전에 배열 크기를 확인하는 방어적 프로그래밍(defensive programming)이 필요하다.

if (msg->ranges.size() > target_index)
{
    float value = msg->ranges[target_index];
    // 조건 평가 수행
}

6.4 성능 최적화

다중 필드를 비교하는 조건 노드에서는 단락 평가(short-circuit evaluation)를 적용하여 불필요한 연산을 회피할 수 있다. 논리 AND 조건에서 첫 번째 비교가 false로 평가되면, 나머지 비교를 수행하지 않고 즉시 FAILURE를 반환한다. C++의 && 연산자는 기본적으로 단락 평가를 지원하므로, 조건식을 적절히 구성하면 별도의 최적화 없이도 이 효과를 얻을 수 있다.

7. 참고 문헌

• 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.
• ROS2 공식 문서. https://docs.ros.org/en/humble/
• BehaviorTree.CPP 공식 문서. https://www.behaviortree.dev/