1293.59 비동기 노드의 결과 수집 메커니즘

1293.59 비동기 노드의 결과 수집 메커니즘

1. 결과 수집의 정의

비동기 노드의 결과 수집(result collection)이란, 외부 시스템이나 별도 스레드에서 수행된 작업의 완료 상태와 결과 데이터를 Tick 스레드로 가져오는 과정을 의미한다. 비동기 노드는 작업을 외부에 위임하고 RUNNING을 반환하므로, 후속 Tick에서 작업의 완료 여부를 확인하고 결과를 수집하여 SUCCESS 또는 FAILURE로 변환해야 한다(Faconti, 2022).

2. 결과 수집의 시점

결과 수집은 비동기 노드가 RUNNING 상태에서 재Tick될 때 onRunning() 메서드 내에서 수행된다. 매 Tick마다 외부 작업의 상태를 폴링(polling)하여 완료 여부를 판정한다.

Tick N:   onStart() → 작업 위임 → RUNNING 반환
Tick N+1: onRunning() → 상태 확인 → 미완료 → RUNNING 반환
Tick N+2: onRunning() → 상태 확인 → 미완료 → RUNNING 반환
Tick N+3: onRunning() → 상태 확인 → 완료 감지 → 결과 수집 → SUCCESS/FAILURE 반환

3. 폴링 기반 결과 수집

폴링 기반 결과 수집은 매 Tick마다 외부 작업의 상태를 능동적으로 조회하는 방식이다. 이는 행동 트리의 Tick 메커니즘과 자연스럽게 결합되는 기본적 수집 방식이다.

3.1 std::future를 이용한 결과 수집

C++ 표준 라이브러리의 std::future를 사용하여 비동기 작업의 결과를 수집할 수 있다.

class AsyncComputation : public BT::StatefulActionNode {
public:
    BT::NodeStatus onStart() override {
        future_ = std::async(std::launch::async, [this]() {
            return performHeavyComputation();
        });
        return BT::NodeStatus::RUNNING;
    }

    BT::NodeStatus onRunning() override {
        // 비차단 폴링: 즉시 완료 여부 확인
        if (future_.wait_for(std::chrono::milliseconds(0)) 
            == std::future_status::ready) {
            // 결과 수집
            auto result = future_.get();
            setOutput("output", result);
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::RUNNING;
    }

    void onHalted() override {
        // future 소멸 시 스레드 조인 발생
    }

private:
    std::future<double> future_;
};

wait_for(0ms)는 비차단 확인으로서, 작업이 완료되지 않았으면 즉시 std::future_status::timeout을 반환하고, 완료되었으면 std::future_status::ready를 반환한다. 이에 의해 Tick 스레드가 차단되지 않는다.

3.2 ROS2 액션 클라이언트를 이용한 결과 수집

ROS2 액션 클라이언트는 비동기 목표 전송 후 결과 콜백을 통해 결과를 수집한다. StatefulActionNode에서는 목표 핸들의 상태를 폴링하여 결과를 수집한다.

class NavigateAction : public BT::StatefulActionNode {
public:
    BT::NodeStatus onStart() override {
        auto goal = NavigateToPose::Goal();
        getInput("target_pose", goal.pose);
        
        auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
        send_goal_options.result_callback = 
            [this](const%20auto&%20result) {
                result_code_ = result.code;
                result_received_ = true;
            };
        
        goal_handle_future_ = action_client_->async_send_goal(goal, send_goal_options);
        result_received_ = false;
        return BT::NodeStatus::RUNNING;
    }

    BT::NodeStatus onRunning() override {
        if (result_received_) {
            // 결과 수집 완료
            return (result_code_ == rclcpp_action::ResultCode::SUCCEEDED)
                ? BT::NodeStatus::SUCCESS
                : BT::NodeStatus::FAILURE;
        }
        return BT::NodeStatus::RUNNING;
    }

    void onHalted() override {
        action_client_->async_cancel_goal(goal_handle_);
    }

private:
    std::atomic<bool> result_received_{false};
    rclcpp_action::ResultCode result_code_;
};

4. 콜백 기반 결과 수집

콜백 기반 결과 수집은 외부 시스템이 작업 완료 시 콜백을 호출하여 결과를 전달하는 방식이다. 결과는 공유 변수에 저장되고, onRunning()에서 해당 변수를 확인한다.

외부 스레드/콜백:
  작업 완료 → 콜백 호출 → 공유 변수에 결과 저장 → 완료 플래그 설정

Tick 스레드 (onRunning):
  완료 플래그 확인 → 설정됨 → 공유 변수에서 결과 읽기 → SUCCESS/FAILURE 반환

이 방식에서 공유 변수와 완료 플래그에 대한 스레드 안전성이 보장되어야 한다. std::atomic<bool>을 완료 플래그로 사용하고, 결과 데이터의 쓰기가 플래그 설정 이전에 완료되도록 메모리 순서(memory ordering)를 보장해야 한다.

5. 공유 플래그 기반 결과 수집

외부 스레드에서 수행되는 작업의 결과를 원자적 플래그를 통해 수집하는 패턴이다.

class ThreadedTask : public BT::StatefulActionNode {
public:
    BT::NodeStatus onStart() override {
        is_done_ = false;
        success_ = false;
        
        worker_thread_ = std::thread([this]() {
            bool result = executeTask();
            success_.store(result, std::memory_order_release);
            is_done_.store(true, std::memory_order_release);
        });
        worker_thread_.detach();
        return BT::NodeStatus::RUNNING;
    }

    BT::NodeStatus onRunning() override {
        if (is_done_.load(std::memory_order_acquire)) {
            return success_.load(std::memory_order_acquire)
                ? BT::NodeStatus::SUCCESS
                : BT::NodeStatus::FAILURE;
        }
        return BT::NodeStatus::RUNNING;
    }

    void onHalted() override {
        cancel_requested_ = true;
    }

private:
    std::atomic<bool> is_done_{false};
    std::atomic<bool> success_{false};
    std::atomic<bool> cancel_requested_{false};
    std::thread worker_thread_;
};

6. 결과 데이터의 블랙보드 전달

결과 수집 시 작업의 결과 데이터를 블랙보드에 기록하여 후속 노드에서 활용할 수 있도록 한다. 결과 데이터의 블랙보드 기록은 onRunning()에서 SUCCESS 또는 FAILURE를 반환하기 직전에 수행한다.

BT::NodeStatus onRunning() override {
    if (result_received_) {
        // 결과 데이터를 블랙보드에 기록
        setOutput("detected_objects", result_data_.objects);
        setOutput("confidence", result_data_.confidence);
        setOutput("processing_time", result_data_.elapsed_ms);
        
        return (result_data_.confidence > 0.8)
            ? BT::NodeStatus::SUCCESS
            : BT::NodeStatus::FAILURE;
    }
    return BT::NodeStatus::RUNNING;
}

7. 결과 수집 실패의 처리

외부 작업이 예외적으로 종료되거나 통신이 단절되어 결과를 수집할 수 없는 경우가 존재한다. 이러한 상황에서는 타임아웃 메커니즘을 적용하여 무한 RUNNING 상태를 방지해야 한다.

BT::NodeStatus onStart() override {
    start_time_ = std::chrono::steady_clock::now();
    // 작업 개시
    return BT::NodeStatus::RUNNING;
}

BT::NodeStatus onRunning() override {
    auto elapsed = std::chrono::steady_clock::now() - start_time_;
    if (elapsed > timeout_duration_) {
        // 타임아웃: 결과 수집 실패
        return BT::NodeStatus::FAILURE;
    }
    
    if (result_received_) {
        return processResult();
    }
    return BT::NodeStatus::RUNNING;
}

8. 결과 수집 방식의 비교

방식구현 복잡도스레드 안전성Tick 차단적용 대상
std::future 폴링낮음보장됨없음단일 비동기 계산
ROS2 액션 결과 콜백중간콜백 내 주의없음ROS2 액션 서버
공유 플래그중간원자적 변수 필요없음사용자 정의 스레드
ROS2 서비스 응답낮음콜백 내 주의없음경량 요청-응답

9. 폴링 비용과 Tick 주기의 관계

결과 수집의 폴링은 매 Tick마다 수행되므로, 폴링 비용이 Tick 실행 시간에 누적된다. 트리 내의 RUNNING 상태 비동기 노드 수가 K이고, 각 노드의 폴링 비용이 T_{poll_i}일 때 총 폴링 비용은 다음과 같다.

T_{polling\_total} = \sum_{i=1}^{K} T_{poll_i}

각 노드의 폴링 비용은 원자적 변수 확인(T_{poll} \approx \text{ns}), future 상태 확인(T_{poll} \approx \text{μs}), ROS2 상태 조회(T_{poll} \approx \text{μs}) 수준이므로, 통상적인 트리 규모에서 총 폴링 비용은 Tick 예산의 극히 일부를 차지한다.


참고 문헌

  • Colledanchise, M., & Ogren, P. (2018). Behavior Trees in Robotics and AI: An Introduction. CRC Press.
  • Faconti, D. (2022). BehaviorTree.CPP documentation and API reference. https://www.behaviortree.dev/