비상 정지 상태 확인 조건 노드 (Emergency Stop State Check Condition Node)

1. 개요

비상 정지 상태 확인 조건 노드는 로봇 시스템의 비상 정지(Emergency Stop, E-stop) 장치가 활성화되었는지를 판정하는 조건 노드이다. 비상 정지는 로봇의 모든 동작을 즉시 중단시키는 최고 우선순위의 안전 메커니즘으로, 행동 트리에서 이 상태를 감시하여 소프트웨어 수준의 보호 행동을 병행하는 것이 필수적이다.

2. 비상 정지 시스템의 구조

2.1 하드웨어와 소프트웨어 비상 정지

하드웨어 비상 정지는 물리적 버튼을 누르면 전기 회로를 직접 차단하여 모터 전원을 끊는 방식이며, 소프트웨어 상태와 무관하게 동작한다. 소프트웨어 비상 정지는 하드웨어 비상 정지를 보완하는 역할을 수행한다.

3. ROS2 비상 정지 메시지

비상 정지 상태는 std_msgs::msg::Bool 또는 사용자 정의 메시지를 통해 발행된다.

class IsNotEmergencyStopped
    : public BT::RosTopicSubNode<std_msgs::msg::Bool>
{
public:
    IsNotEmergencyStopped(const std::string& name,
                          const BT::NodeConfiguration& config,
                          const BT::RosNodeParams& params)
        : RosTopicSubNode(name, config, params),
          node_(params.nh),
          last_received_(params.nh->get_clock()->now()),
          ever_received_(false)
    {}

    static BT::PortsList providedPorts()
    {
        return providedBasicPorts({
            BT::InputPort<double>("timeout_sec", 1.0,
                "E-stop 토픽 수신 타임아웃 (초)")
        });
    }

    BT::NodeStatus onTick(
        const std_msgs::msg::Bool::SharedPtr& msg) override
    {
        double timeout;
        getInput("timeout_sec", timeout);

        if (msg)
        {
            last_received_ = node_->get_clock()->now();
            ever_received_ = true;

            // msg->data == true: 비상 정지 활성화
            // msg->data == false: 정상 상태
            if (!msg->data)
            {
                return BT::NodeStatus::SUCCESS;
            }
            return BT::NodeStatus::FAILURE;
        }

        // 메시지 미수신: 타임아웃 확인
        if (!ever_received_)
        {
            return BT::NodeStatus::FAILURE;
        }

        auto elapsed =
            (node_->get_clock()->now() - last_received_).seconds();
        if (elapsed > timeout)
        {
            // 타임아웃: 안전하지 않은 것으로 판정
            return BT::NodeStatus::FAILURE;
        }

        return BT::NodeStatus::SUCCESS;
    }

private:
    rclcpp::Node::SharedPtr node_;
    rclcpp::Time last_received_;
    bool ever_received_;
};

비상 정지 토픽이 수신되지 않는 경우(통신 장애)에도 FAILURE를 반환하여, 실패 안전 원칙을 준수한다. 비상 정지 메시지의 의미론은 data == true가 비상 정지 활성화, data == false가 정상 상태를 나타내는 것이 일반적이다.

4. diagnostic_msgs 기반 구현

로봇 시스템의 진단 정보를 통해 비상 정지 상태를 확인하는 방식이다.

class IsNotEStopFromDiagnostics
    : public BT::RosTopicSubNode<diagnostic_msgs::msg::DiagnosticArray>
{
public:
    IsNotEStopFromDiagnostics(
        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>("hardware_id",
                "비상 정지 장치 하드웨어 ID")
        });
    }

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

        std::string hw_id;
        getInput("hardware_id", hw_id);

        for (const auto& status : msg->status)
        {
            if (status.hardware_id == hw_id)
            {
                if (status.level ==
                    diagnostic_msgs::msg::DiagnosticStatus::ERROR)
                {
                    return BT::NodeStatus::FAILURE;
                }
                return BT::NodeStatus::SUCCESS;
            }
        }
        return BT::NodeStatus::FAILURE;
    }
};

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

5.1 최상위 안전 가드

<BehaviorTree ID="SafeOperation">
    <ReactiveSequence>
        <Condition ID="IsNotEmergencyStopped"
                   topic_name="/e_stop"
                   timeout_sec="0.5"/>
        <SubTree ID="MissionExecution"/>
    </ReactiveSequence>
</BehaviorTree>

5.2 비상 정지 해제 대기

<BehaviorTree ID="EStopRecovery">
    <Fallback>
        <Sequence>
            <Condition ID="IsNotEmergencyStopped"
                       topic_name="/e_stop"/>
            <Action ID="ResumeOperation"/>
        </Sequence>
        <Action ID="WaitForEStopRelease"/>
    </Fallback>
</BehaviorTree>

비상 정지가 해제되면(IsNotEmergencyStopped가 SUCCESS) 운용을 재개하고, 해제되지 않은 동안에는 대기 행동을 유지한다.

6. 설계 시 고려 사항

6.1 비상 정지 해제 후 자동 재개 방지

비상 정지가 해제된 직후 로봇이 자동으로 이전 동작을 재개하는 것은 안전상 위험하다. 비상 정지 해제 후에는 운용자의 명시적 재개 명령을 대기하거나, 시스템 상태를 점검한 후 재개하는 절차가 필요하다.

6.2 비상 정지 래칭

비상 정지가 순간적으로 활성화되었다가 해제된 경우에도, 소프트웨어 수준에서 이 이벤트를 래칭(latching)하여 기록하고, 운용자의 확인 없이는 정상 운용으로 복귀하지 않도록 한다.

6.3 다중 비상 정지 소스

대형 로봇 시스템에는 복수의 비상 정지 장치(본체 버튼, 원격 버튼, 소프트웨어 E-stop)가 존재할 수 있다. 이 경우 모든 소스의 상태를 종합하여, 어느 하나라도 활성화되어 있으면 비상 정지 상태로 판정하여야 한다.

7. 참고 문헌

• IEC 60204-1: Safety of machinery — Electrical equipment of machines.
• ISO 13850:2015. Safety of machinery — Emergency stop function.
• Colledanchise, M., & Ogren, P. (2018). Behavior Trees in Robotics and AI: An Introduction. CRC Press.
• BehaviorTree.CPP 공식 문서. https://www.behaviortree.dev/