시스템 건강 상태 확인 조건 노드 (System Health State Check Condition Node)

1. 개요

시스템 건강 상태 확인 조건 노드는 로봇 시스템의 전반적인 건강 상태(system health)를 평가하여, 모든 하위 시스템이 정상적으로 동작하고 있는지를 판정하는 조건 노드이다. ROS2의 진단(diagnostics) 프레임워크를 활용하여 센서, 제어기, 통신, 전원 등 각 구성 요소의 상태를 종합적으로 감시하며, 이상이 감지되면 임무를 제한하거나 정지시켜 시스템의 안전한 운용을 보장한다.

2. ROS2 진단 프레임워크

2.1 diagnostic_msgs::msg::DiagnosticArray

ROS2에서 시스템 진단 정보는 /diagnostics 토픽을 통해 DiagnosticArray 메시지로 발행된다.

2.2 diagnostic_msgs::msg::DiagnosticStatus

상태 수준 상수는 다음과 같다.

3. 구현

3.1 전체 시스템 건강 확인

모든 진단 항목이 OK 상태인지를 확인한다.

class IsSystemHealthy
    : public BT::RosTopicSubNode<diagnostic_msgs::msg::DiagnosticArray>
{
public:
    IsSystemHealthy(const std::string& name,
                    const BT::NodeConfiguration& config,
                    const BT::RosNodeParams& params)
        : RosTopicSubNode(name, config, params),
          node_(params.nh)
    {}

    static BT::PortsList providedPorts()
    {
        return providedBasicPorts({
            BT::InputPort<int>("max_level", 0,
                "최대 허용 진단 수준 (0=OK, 1=WARN)"),
            BT::InputPort<double>("max_age_sec", 10.0,
                "진단 데이터 최대 유효 시간 (초)")
        });
    }

    BT::NodeStatus onTick(
        const diagnostic_msgs::msg::DiagnosticArray::SharedPtr& msg)
        override
    {
        if (!msg || msg->status.empty())
        {
            return BT::NodeStatus::FAILURE;
        }

        int max_level;
        double max_age;
        getInput("max_level", max_level);
        getInput("max_age_sec", max_age);

        // 메시지 신선도 확인
        auto now = node_->get_clock()->now();
        auto msg_time = rclcpp::Time(msg->header.stamp);
        double age = (now - msg_time).seconds();
        if (age > max_age)
        {
            return BT::NodeStatus::FAILURE;
        }

        for (const auto& status : msg->status)
        {
            if (status.level > static_cast<byte>(max_level))
            {
                return BT::NodeStatus::FAILURE;
            }
        }
        return BT::NodeStatus::SUCCESS;
    }

private:
    rclcpp::Node::SharedPtr node_;
};

max_level을 0으로 설정하면 모든 항목이 OK여야 하며, 1로 설정하면 WARN까지 허용한다.

3.2 특정 구성 요소 건강 확인

특정 하드웨어 또는 소프트웨어 구성 요소의 진단 상태만을 확인한다.

class IsComponentHealthy
    : public BT::RosTopicSubNode<diagnostic_msgs::msg::DiagnosticArray>
{
public:
    IsComponentHealthy(const std::string& name,
                       const BT::NodeConfiguration& config,
                       const BT::RosNodeParams& params)
        : RosTopicSubNode(name, config, params)
    {}

    static BT::PortsList providedPorts()
    {
        return providedBasicPorts({
            BT::InputPort<std::string>("component_name",
                "확인할 구성 요소 이름"),
            BT::InputPort<int>("max_level", 0,
                "최대 허용 수준")
        });
    }

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

        std::string component;
        int max_level;
        getInput("component_name", component);
        getInput("max_level", max_level);

        for (const auto& status : msg->status)
        {
            if (status.name == component ||
                status.hardware_id == component)
            {
                if (status.level <= static_cast<byte>(max_level))
                {
                    return BT::NodeStatus::SUCCESS;
                }
                return BT::NodeStatus::FAILURE;
            }
        }
        // 구성 요소를 찾지 못함
        return BT::NodeStatus::FAILURE;
    }
};

4. diagnostic_aggregator 연동

ROS2의 diagnostic_aggregator 패키지는 개별 진단 메시지를 수집하여 계층적으로 집계한다. /diagnostics_agg 토픽에서 집계된 진단 정보를 구독하면, 개별 구성 요소의 상태를 분류별로 확인할 수 있다.

<BehaviorTree ID="AggregatedHealthCheck">
    <ReactiveSequence>
        <Condition ID="IsComponentHealthy"
                   topic_name="/diagnostics_agg"
                   component_name="/Sensors"
                   max_level="0"/>
        <Condition ID="IsComponentHealthy"
                   topic_name="/diagnostics_agg"
                   component_name="/Motors"
                   max_level="1"/>
        <Action ID="ContinueOperation"/>
    </ReactiveSequence>
</BehaviorTree>

센서는 OK 상태만 허용하고, 모터는 WARN까지 허용하는 차등적 건강 기준을 적용한다.

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

5.1 시스템 건강 기반 운용 모드 선택

<BehaviorTree ID="HealthBasedOperation">
    <Fallback>
        <Sequence>
            <Condition ID="IsSystemHealthy"
                       topic_name="/diagnostics"
                       max_level="0"/>
            <Action ID="FullCapabilityOperation"/>
        </Sequence>
        <Sequence>
            <Condition ID="IsSystemHealthy"
                       topic_name="/diagnostics"
                       max_level="1"/>
            <Action ID="DegradedOperation"/>
        </Sequence>
        <Action ID="SafeShutdown"/>
    </Fallback>
</BehaviorTree>

모든 구성 요소가 정상이면 전체 기능으로 운용하고, 경고가 있으면 기능을 제한하며, 오류가 있으면 안전 종료를 수행한다.

6. 설계 시 고려 사항

6.1 진단 발행 주기

각 구성 요소의 진단 발행 주기가 다를 수 있다. 발행 주기가 긴 구성 요소의 진단 정보가 STALE 상태로 전환되지 않도록, 적절한 타임아웃을 설정하여야 한다.

6.2 진단 정보의 포괄성

시스템의 모든 중요 구성 요소가 진단 정보를 발행하도록 구성하여야 한다. 진단 정보를 발행하지 않는 구성 요소는 건강 상태 확인의 사각 지대가 된다.

6.3 자체 진단의 한계

소프트웨어 기반 자체 진단(self-diagnostics)은 소프트웨어 자체의 오류를 감지하지 못할 수 있다. 외부 감시(watchdog timer) 메커니즘을 병행하여 시스템의 전반적 건전성을 보장하여야 한다.

7. 참고 문헌

• Colledanchise, M., & Ogren, P. (2018). Behavior Trees in Robotics and AI: An Introduction. CRC Press.
• ROS2 diagnostics 패키지 문서. https://docs.ros.org/en/humble/
• BehaviorTree.CPP 공식 문서. https://www.behaviortree.dev/