비행 모드 확인 조건 노드 (Flight Mode Check Condition Node)

1. 개요

비행 모드 확인 조건 노드는 드론의 비행 제어기(FCU)가 현재 설정된 비행 모드를 확인하여, 기대하는 모드에 있는지를 판정하는 조건 노드이다. 드론의 비행 모드는 자율 비행, 위치 유지, 수동 조종 등 다양한 제어 방식을 결정하며, 자율 임무를 수행하기 위해서는 적절한 비행 모드(예: OFFBOARD, AUTO.MISSION)가 활성화되어야 한다.

2. PX4 비행 모드 체계

2.1 주요 비행 모드

PX4 비행 제어기에서 지원하는 주요 비행 모드는 다음과 같다.

2.2 MAVROS 상태 메시지

mavros_msgs::msg::State 메시지는 FCU의 현재 상태를 보고한다.

3. 구현

3.1 비행 모드 일치 확인

class IsFlightModeCorrect
    : public BT::RosTopicSubNode<mavros_msgs::msg::State>
{
public:
    IsFlightModeCorrect(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>("expected_mode", "OFFBOARD",
                "기대하는 비행 모드")
        });
    }

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

        if (!msg->connected)
        {
            return BT::NodeStatus::FAILURE;
        }

        std::string expected_mode;
        getInput("expected_mode", expected_mode);

        if (msg->mode == expected_mode)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

3.2 무장 상태 확인

드론의 모터가 무장(armed) 상태인지를 확인하는 조건 노드이다.

class IsArmed
    : public BT::RosTopicSubNode<mavros_msgs::msg::State>
{
public:
    IsArmed(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 mavros_msgs::msg::State::SharedPtr& msg) override
    {
        if (!msg || !msg->connected)
        {
            return BT::NodeStatus::FAILURE;
        }

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

3.3 FCU 연결 상태 확인

class IsFcuConnected
    : public BT::RosTopicSubNode<mavros_msgs::msg::State>
{
public:
    IsFcuConnected(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 mavros_msgs::msg::State::SharedPtr& msg) override
    {
        if (!msg)
        {
            return BT::NodeStatus::FAILURE;
        }
        return msg->connected ? BT::NodeStatus::SUCCESS
                              : BT::NodeStatus::FAILURE;
    }
};

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

4.1 OFFBOARD 모드 전제 조건

<BehaviorTree ID="OffboardFlight">
    <ReactiveSequence>
        <Condition ID="IsFcuConnected"
                   topic_name="/mavros/state"/>
        <Condition ID="IsArmed"
                   topic_name="/mavros/state"/>
        <Condition ID="IsFlightModeCorrect"
                   topic_name="/mavros/state"
                   expected_mode="OFFBOARD"/>
        <Action ID="SendSetpointCommands"/>
    </ReactiveSequence>
</BehaviorTree>

4.2 비행 모드 전환 감지 및 대응

<BehaviorTree ID="ModeAwareMission">
    <Fallback>
        <Sequence>
            <Condition ID="IsFlightModeCorrect"
                       topic_name="/mavros/state"
                       expected_mode="OFFBOARD"/>
            <Action ID="AutonomousMission"/>
        </Sequence>
        <Sequence>
            <Condition ID="IsFlightModeCorrect"
                       topic_name="/mavros/state"
                       expected_mode="AUTO.RTL"/>
            <Action ID="MonitorReturnToLaunch"/>
        </Sequence>
        <Action ID="WaitForModeChange"/>
    </Fallback>
</BehaviorTree>

운용자가 RC 송신기를 통해 비행 모드를 전환하면, 행동 트리는 자동으로 변경된 모드에 맞는 행동을 선택한다.

5. 설계 시 고려 사항

5.1 OFFBOARD 모드의 유지 조건

PX4에서 OFFBOARD 모드는 외부 명령(setpoint)이 일정 주기(기본 2Hz 이상)로 수신되어야 유지된다. 명령이 중단되면 FCU는 자동으로 실패 안전 모드(예: POSCTL 또는 AUTO.LOITER)로 전환한다. 조건 노드에서 이 전환을 감지하여 적절한 복구 행동을 실행하여야 한다.

5.2 모드 문자열의 정확한 일치

비행 모드 이름은 FCU 펌웨어와 MAVROS 버전에 따라 상이할 수 있다. 예를 들어, PX4에서는 "OFFBOARD", ArduPilot에서는 "GUIDED"가 유사한 기능을 수행한다. 사용하는 FCU 플랫폼에 맞는 모드 이름을 정확히 지정하여야 한다.

5.3 RC 오버라이드

운용자가 RC 송신기를 통해 비행 모드를 수동으로 전환하면, 자율 임무가 중단된다. 이 상황에서 행동 트리가 강제로 OFFBOARD 모드를 재설정하는 것은 안전상 부적절하다. RC 오버라이드를 존중하고, 운용자의 수동 제어를 우선시하여야 한다.

6. 참고 문헌

• Meier, L., et al. (2015). “PX4: A Node-Based Multithreaded Open Source Robotics Framework for Deeply Embedded Platforms.” ICRA 2015.
• Colledanchise, M., & Ogren, P. (2018). Behavior Trees in Robotics and AI: An Introduction. CRC Press.
• PX4 공식 문서. https://docs.px4.io/
• MAVROS 공식 문서. http://wiki.ros.org/mavros