1296.40 ROS2 액션 클라이언트 기반 액션 노드 구현

1. ROS2 액션과 행동 트리 액션 노드의 연결

ROS2 액션(Action)은 장시간 실행되는 비동기 작업을 위한 통신 인터페이스로서, 골(Goal) 전송, 피드백(Feedback) 수신, 결과(Result) 획득, 취소(Cancel) 요청의 네 가지 상호 작용을 지원한다. 행동 트리의 액션 노드는 ROS2 액션 클라이언트를 내부에 보유하여, 트리의 Tick에 의해 액션 서버와의 상호 작용을 제어한다. 이 구조에서 행동 트리 액션 노드는 ROS2 액션 클라이언트의 생명주기를 관리하는 래퍼(wrapper)로서 기능한다(Macenski et al., 2020).

ROS2 액션 클라이언트 기반 액션 노드의 핵심 과제는 ROS2 액션의 비동기 통신 모델과 행동 트리의 Tick 기반 실행 모델 간의 조화이다. ROS2 액션 클라이언트는 비동기 콜백을 통해 서버의 응답을 수신하는 반면, 행동 트리 액션 노드는 매 Tick마다 NodeStatus를 반환하여야 한다. 이 두 모델의 연결은 StatefulActionNode의 상태 기반 콜백 구조에 의해 달성된다.

2. 구현의 기반 구조

2.1 StatefulActionNode 기반의 액션 클라이언트 노드

ROS2 액션 클라이언트 기반 액션 노드는 StatefulActionNode를 기반으로 구현한다. StatefulActionNode의 세 가지 콜백이 ROS2 액션 클라이언트의 생명주기에 다음과 같이 대응한다.

2.2 노드 클래스의 기본 구조

template <typename ActionT>
class RosActionNode : public BT::StatefulActionNode
{
public:
    using ActionClient = rclcpp_action::Client<ActionT>;
    using GoalHandle = rclcpp_action::ClientGoalHandle<ActionT>;
    using Goal = typename ActionT::Goal;
    using Result = typename ActionT::Result;
    using Feedback = typename ActionT::Feedback;

    RosActionNode(
        const std::string& name,
        const BT::NodeConfig& config,
        rclcpp::Node::SharedPtr node)
        : BT::StatefulActionNode(name, config)
        , node_(node)
    {
    }

protected:
    rclcpp::Node::SharedPtr node_;
    typename ActionClient::SharedPtr action_client_;
    typename GoalHandle::SharedPtr goal_handle_;
    typename Result::SharedPtr result_;
    bool goal_completed_ = false;
};

이 템플릿 구조에서 ActionT는 ROS2 액션 타입(예: nav2_msgs::action::NavigateToPose)이며, 액션 클라이언트, 골 핸들, 결과 저장소가 멤버 변수로 선언된다.

3. 액션 클라이언트의 초기화

액션 클라이언트의 생성은 노드의 생성자 또는 최초 onStart() 호출 시에 수행한다. 지연 초기화(lazy initialization) 패턴은 액션 서버 이름이 포트를 통해 동적으로 결정되는 경우에 적용된다.

3.1 생성자에서의 초기화

RosActionNode(
    const std::string& name,
    const BT::NodeConfig& config,
    rclcpp::Node::SharedPtr node,
    const std::string& action_name)
    : BT::StatefulActionNode(name, config)
    , node_(node)
{
    action_client_ = rclcpp_action::create_client<ActionT>(
        node_, action_name);
}

3.2 지연 초기화

BT::NodeStatus onStart() override
{
    std::string server_name;
    if (!getInput("server_name", server_name))
        return BT::NodeStatus::FAILURE;

    if (!action_client_ || current_server_ != server_name)
    {
        action_client_ = rclcpp_action::create_client<ActionT>(
            node_, server_name);
        current_server_ = server_name;
    }
    // ...
}

지연 초기화는 포트를 통한 서버 이름 변경을 지원하지만, 매 실행마다 서버 탐색이 필요할 수 있다는 비용을 수반한다.

4. onStart() 콜백의 구현

onStart()는 액션 노드가 최초로 Tick될 때 호출되며, 다음의 단계를 수행한다.

4.1 단계별 처리 과정

onStart()
    │
    ├─ 1. 입력 포트 읽기
    │       └─ getInput()으로 골 파라미터 획득
    │
    ├─ 2. 액션 서버 가용성 확인
    │       └─ wait_for_action_server() (짧은 타임아웃)
    │
    ├─ 3. 골 메시지 구성
    │       └─ 입력 포트 값으로 Goal 메시지 생성
    │
    ├─ 4. 골 전송
    │       └─ async_send_goal() (비동기)
    │
    └─ 5. 반환
            ├─ 서버 미가용 → FAILURE
            ├─ 골 전송 성공 → RUNNING
            └─ 골 전송 실패 → FAILURE

4.2 구현 코드

BT::NodeStatus onStart() override
{
    // 1. 입력 포트 읽기
    Goal goal;
    if (!populateGoal(goal))
        return BT::NodeStatus::FAILURE;

    // 2. 액션 서버 가용성 확인
    if (!action_client_->wait_for_action_server(
            std::chrono::milliseconds(100)))
    {
        RCLCPP_ERROR(node_->get_logger(),
            "액션 서버 '%s' 미가용", current_server_.c_str());
        return BT::NodeStatus::FAILURE;
    }

    // 3. 골 전송 옵션 설정
    auto send_goal_options =
        typename rclcpp_action::Client<ActionT>::SendGoalOptions();

    send_goal_options.result_callback =
        [this](const%20typename%20GoalHandle::WrappedResult&%20result)
        {
            result_ = std::make_shared<Result>(result.result);
            goal_result_code_ = result.code;
            goal_completed_ = true;
        };

    send_goal_options.feedback_callback =
        [this](typename%20GoalHandle::SharedPtr,
%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20const%20std::shared_ptr<const%20Feedback>%20feedback)
        {
            latest_feedback_ = feedback;
        };

    // 4. 골 전송
    auto goal_handle_future =
        action_client_->async_send_goal(goal, send_goal_options);

    // 5. 골 수락 대기 (짧은 시간)
    if (rclcpp::spin_until_future_complete(
            node_, goal_handle_future,
            std::chrono::milliseconds(100))
        != rclcpp::FutureReturnCode::SUCCESS)
    {
        RCLCPP_ERROR(node_->get_logger(), "골 전송 실패");
        return BT::NodeStatus::FAILURE;
    }

    goal_handle_ = goal_handle_future.get();
    if (!goal_handle_)
    {
        RCLCPP_ERROR(node_->get_logger(), "골이 서버에 의해 거부됨");
        return BT::NodeStatus::FAILURE;
    }

    // 상태 초기화
    goal_completed_ = false;
    result_ = nullptr;
    latest_feedback_ = nullptr;

    return BT::NodeStatus::RUNNING;
}

onStart()에서 골 전송 후 즉시 RUNNING을 반환함으로써, 행동 트리의 메인 스레드를 차단하지 않는다. 골의 수락 대기에는 짧은 타임아웃을 적용하여, 서버의 즉각적 응답만을 확인한다.

5. onRunning() 콜백의 구현

onRunning()은 RUNNING 상태에서 매 Tick마다 호출되며, ROS2 액션의 진행 상태를 확인하고 완료 여부를 판단한다.

5.1 구현 코드

BT::NodeStatus onRunning() override
{
    // ROS2 콜백 처리
    rclcpp::spin_some(node_);

    // 완료 확인
    if (goal_completed_)
    {
        return processResult();
    }

    // 피드백 기반 출력 갱신 (선택적)
    if (latest_feedback_)
    {
        processFeedback(latest_feedback_);
    }

    // 타임아웃 검사
    if (isTimedOut())
    {
        RCLCPP_WARN(node_->get_logger(), "액션 타임아웃");
        cancelGoal();
        return BT::NodeStatus::FAILURE;
    }

    return BT::NodeStatus::RUNNING;
}

rclcpp::spin_some()의 호출은 ROS2 콜백 큐에 대기 중인 콜백을 처리하기 위해 필요하다. 이 호출에 의해 result_callback과 feedback_callback이 실행되어, 멤버 변수 goal_completed_와 latest_feedback_가 갱신된다.

5.2 결과 처리

BT::NodeStatus processResult()
{
    using rclcpp_action::ResultCode;

    switch (goal_result_code_)
    {
    case ResultCode::SUCCEEDED:
        onResultReceived(*result_);
        return BT::NodeStatus::SUCCESS;

    case ResultCode::ABORTED:
        RCLCPP_WARN(node_->get_logger(), "액션이 서버에 의해 중단됨");
        setOutput("error_code", getErrorCodeFromResult(*result_));
        return BT::NodeStatus::FAILURE;

    case ResultCode::CANCELED:
        RCLCPP_INFO(node_->get_logger(), "액션이 취소됨");
        return BT::NodeStatus::FAILURE;

    default:
        RCLCPP_ERROR(node_->get_logger(), "알 수 없는 결과 코드");
        return BT::NodeStatus::FAILURE;
    }
}

ResultCode에 따른 NodeStatus 매핑은 ROS2 액션의 완료 상태를 행동 트리의 반환 상태로 변환하는 핵심 로직이다. SUCCEEDED는 SUCCESS로, ABORTED와 CANCELED는 FAILURE로 매핑된다.

6. onHalted() 콜백의 구현

onHalted()는 행동 트리가 액션 노드의 실행을 중단할 때 호출되며, 진행 중인 ROS2 액션 골을 취소하여야 한다.

void onHalted() override
{
    cancelGoal();
}

void cancelGoal()
{
    if (goal_handle_)
    {
        auto cancel_future =
            action_client_->async_cancel_goal(goal_handle_);

        rclcpp::spin_until_future_complete(
            node_, cancel_future,
            std::chrono::milliseconds(500));

        goal_handle_.reset();
    }
    goal_completed_ = false;
    result_ = nullptr;
    latest_feedback_ = nullptr;
}

골 취소 요청 후 짧은 시간 동안 취소 확인을 대기하되, onHalted()가 과도하게 차단되지 않도록 타임아웃을 설정한다. 골 핸들의 초기화와 상태 변수의 정리가 필수적이다.

7. 골 메시지 구성 패턴

입력 포트의 값으로 ROS2 액션 골 메시지를 구성하는 패턴을 기술한다.

7.1 직접 매핑 패턴

포트 값을 골 메시지의 필드에 직접 대입한다.

bool populateGoal(Goal& goal)
{
    geometry_msgs::msg::PoseStamped target;
    if (!getInput("goal_pose", target))
    {
        RCLCPP_ERROR(node_->get_logger(),
            "필수 입력 'goal_pose' 누락");
        return false;
    }

    goal.pose = target;

    std::string behavior_tree;
    getInput("behavior_tree", behavior_tree);
    goal.behavior_tree = behavior_tree;

    return true;
}

7.2 가상 함수 패턴

구체적인 골 구성을 파생 클래스에 위임한다.

// 기반 클래스
virtual bool populateGoal(Goal& goal) = 0;
virtual void onResultReceived(const Result& result) {}
virtual void processFeedback(
    const std::shared_ptr<const Feedback>& feedback) {}

// 파생 클래스: NavigateToPose
class NavigateToPoseAction
    : public RosActionNode<nav2_msgs::action::NavigateToPose>
{
    bool populateGoal(Goal& goal) override
    {
        geometry_msgs::msg::PoseStamped pose;
        if (!getInput("goal_pose", pose))
            return false;

        goal.pose = pose;

        std::string bt;
        getInput("behavior_tree", bt);
        goal.behavior_tree = bt;

        return true;
    }

    void onResultReceived(const Result& result) override
    {
        setOutput("error_code", result.error_code);
    }
};

가상 함수 패턴은 템플릿 기반 클래스의 재사용성을 극대화하며, 새로운 액션 타입에 대한 노드 구현을 최소한의 코드로 가능하게 한다.

8. 포트 설계

ROS2 액션 클라이언트 기반 노드의 포트 설계는 액션의 골, 피드백, 결과에 대응하는 구조를 따른다.

static BT::PortsList providedPorts()
{
    return {
        // 골 관련 입력 포트 (필수)
        BT::InputPort<geometry_msgs::msg::PoseStamped>(
            "goal_pose", "목표 위치와 자세"),

        // 구성 입력 포트 (선택)
        BT::InputPort<std::string>(
            "server_name", "navigate_to_pose",
            "액션 서버 이름"),
        BT::InputPort<double>(
            "timeout_sec", "30.0",
            "타임아웃 (초)"),

        // 결과 출력 포트 (선택)
        BT::OutputPort<int>(
            "error_code", "오류 코드"),

        // 피드백 출력 포트 (선택)
        BT::OutputPort<double>(
            "distance_remaining", "남은 거리")
    };
}

9. spin_some()의 통합

onRunning()에서 rclcpp::spin_some()을 호출하는 것은 ROS2 콜백의 처리를 위해 필수적이나, 이 호출에는 주의사항이 존재한다.

9.1 단일 실행기 제약

노드가 이미 외부 실행기(executor)에 의해 스핀되고 있는 경우, onRunning() 내부에서 spin_some()을 호출하면 충돌이 발생한다. 이 경우 콜백 그룹(callback group)의 분리 또는 별도의 콜백 실행기를 사용한다.

// 콜백 그룹 분리 패턴
RosActionNode(
    const std::string& name,
    const BT::NodeConfig& config,
    rclcpp::Node::SharedPtr node)
    : BT::StatefulActionNode(name, config)
    , node_(node)
{
    callback_group_ = node_->create_callback_group(
        rclcpp::CallbackGroupType::MutuallyExclusive);

    action_client_ = rclcpp_action::create_client<ActionT>(
        node_, action_name,
        callback_group_);
}

9.2 Nav2의 spin_some() 관리

Nav2 프레임워크에서는 행동 트리 실행기(BT executor)가 트리의 Tick 루프 내에서 spin_some()을 호출하여, 모든 액션 노드의 ROS2 콜백을 일괄적으로 처리한다. 이 접근에서는 개별 액션 노드가 spin_some()을 호출할 필요가 없다(Macenski et al., 2020).

// Nav2 BT 실행기의 Tick 루프 (개념적 구조)
while (running)
{
    rclcpp::spin_some(node_);
    auto status = tree_.tickOnce();

    if (status == BT::NodeStatus::SUCCESS ||
        status == BT::NodeStatus::FAILURE)
    {
        break;
    }

    loop_rate.sleep();
}

10. 전체 구현 사례

내비게이션 액션 노드의 전체 구현을 제시한다.

class NavigateToPoseAction
    : public BT::StatefulActionNode
{
public:
    using NavigateToPose = nav2_msgs::action::NavigateToPose;
    using GoalHandle =
        rclcpp_action::ClientGoalHandle<NavigateToPose>;

    NavigateToPoseAction(
        const std::string& name,
        const BT::NodeConfig& config,
        rclcpp::Node::SharedPtr node)
        : BT::StatefulActionNode(name, config)
        , node_(node)
    {
        action_client_ =
            rclcpp_action::create_client<NavigateToPose>(
                node_, "navigate_to_pose");
    }

    static BT::PortsList providedPorts()
    {
        return {
            BT::InputPort<geometry_msgs::msg::PoseStamped>(
                "goal", "목표 위치와 자세"),
            BT::InputPort<std::string>(
                "server_name", "navigate_to_pose",
                "액션 서버 이름"),
            BT::OutputPort<int>(
                "error_code", "내비게이션 오류 코드")
        };
    }

    BT::NodeStatus onStart() override
    {
        geometry_msgs::msg::PoseStamped goal_pose;
        if (!getInput("goal", goal_pose))
        {
            RCLCPP_ERROR(node_->get_logger(),
                "필수 입력 'goal' 누락");
            return BT::NodeStatus::FAILURE;
        }

        if (!action_client_->wait_for_action_server(
                std::chrono::milliseconds(100)))
        {
            RCLCPP_ERROR(node_->get_logger(),
                "내비게이션 액션 서버 미가용");
            return BT::NodeStatus::FAILURE;
        }

        auto goal_msg = NavigateToPose::Goal();
        goal_msg.pose = goal_pose;

        auto options =
            rclcpp_action::Client<NavigateToPose>
                ::SendGoalOptions();

        options.result_callback =
            [this](const%20GoalHandle::WrappedResult&%20result)
            {
                result_ = result.result;
                result_code_ = result.code;
                goal_done_ = true;
            };

        auto future =
            action_client_->async_send_goal(goal_msg, options);

        if (rclcpp::spin_until_future_complete(
                node_, future,
                std::chrono::milliseconds(100))
            != rclcpp::FutureReturnCode::SUCCESS)
        {
            return BT::NodeStatus::FAILURE;
        }

        goal_handle_ = future.get();
        if (!goal_handle_)
            return BT::NodeStatus::FAILURE;

        goal_done_ = false;
        return BT::NodeStatus::RUNNING;
    }

    BT::NodeStatus onRunning() override
    {
        if (!goal_done_)
            return BT::NodeStatus::RUNNING;

        using rclcpp_action::ResultCode;
        if (result_code_ == ResultCode::SUCCEEDED)
        {
            setOutput("error_code", 0);
            return BT::NodeStatus::SUCCESS;
        }

        if (result_)
        {
            setOutput("error_code", result_->error_code);
        }
        return BT::NodeStatus::FAILURE;
    }

    void onHalted() override
    {
        if (goal_handle_)
        {
            action_client_->async_cancel_goal(goal_handle_);
            goal_handle_.reset();
        }
        goal_done_ = false;
    }

private:
    rclcpp::Node::SharedPtr node_;
    rclcpp_action::Client<NavigateToPose>::SharedPtr
        action_client_;
    GoalHandle::SharedPtr goal_handle_;
    NavigateToPose::Result::SharedPtr result_;
    rclcpp_action::ResultCode result_code_;
    bool goal_done_ = false;
};

11. ROS2 액션 상태와 행동 트리 반환 상태의 매핑

12. 설계 지침

1. 템플릿 기반 클래스의 활용: 액션 타입을 템플릿 파라미터로 추상화하여, 공통 로직(골 전송, 결과 대기, 취소 처리)을 기반 클래스에 집중하고, 골 구성과 결과 해석을 가상 함수로 파생 클래스에 위임한다.

2. onStart()에서의 비차단 골 전송: 골 전송과 수락 확인에 짧은 타임아웃을 적용하여, 행동 트리의 메인 스레드가 장시간 차단되지 않도록 한다.

3. onHalted()에서의 골 취소 보장: 행동 트리가 액션 노드를 중단할 때 진행 중인 ROS2 액션 골을 반드시 취소하여, 서버 측 자원의 누수를 방지한다.

4. 콜백 기반 상태 갱신: result_callback과 feedback_callback을 통해 멤버 변수를 갱신하고, onRunning()에서 이 변수를 확인하는 패턴을 사용하여, 비동기 통신과 Tick 기반 실행의 조화를 달성한다.

5. 포트 설계의 ROS2 액션 대응: 골의 필드를 입력 포트로, 결과의 필드를 출력 포트로, 피드백의 필드를 선택적 출력 포트로 매핑하여, 행동 트리의 블랙보드를 통한 데이터 흐름을 지원한다(Macenski et al., 2020; Faconti, 2022).