1296.41 ROS2 액션 골 (Goal) 전송 액션 노드

1. 골 전송의 개요

ROS2 액션 클라이언트 기반 행동 트리 액션 노드에서 골(Goal) 전송은 노드가 최초로 Tick될 때 수행되는 핵심 동작이다. 골 전송은 블랙보드의 입력 포트로부터 파라미터를 읽어 ROS2 액션 골 메시지를 구성하고, 이를 액션 서버에 비동기적으로 전달하는 과정으로 이루어진다. 이 과정은 StatefulActionNode의 onStart() 콜백 내에서 완결되며, 골 전송의 성공 여부에 따라 RUNNING 또는 FAILURE를 반환한다(Macenski et al., 2020).

2. 골 전송의 단계별 과정

골 전송은 다음의 다섯 단계로 구성된다.

골 전송 과정
    │
    ├─ 1단계: 입력 포트 읽기
    │       │
    │       ├─ getInput()으로 골 파라미터 획득
    │       │
    │       └─ 필수 입력 누락 → FAILURE 반환
    │
    ├─ 2단계: 액션 서버 가용성 확인
    │       │
    │       ├─ wait_for_action_server() 호출
    │       │
    │       └─ 서버 미가용 → FAILURE 반환
    │
    ├─ 3단계: 골 메시지 구성
    │       │
    │       └─ 입력 포트 값 → Goal 메시지 필드 매핑
    │
    ├─ 4단계: 콜백 설정 및 골 전송
    │       │
    │       ├─ SendGoalOptions 구성
    │       │
    │       └─ async_send_goal() 호출
    │
    └─ 5단계: 골 수락 확인
            │
            ├─ 골 핸들 획득 → RUNNING 반환
            │
            ├─ 골 거부 → FAILURE 반환
            │
            └─ 전송 실패 → FAILURE 반환

3. 단계: 입력 포트로부터 골 파라미터 읽기

골 메시지의 구성에 필요한 데이터를 블랙보드의 입력 포트로부터 읽는다. 필수 입력의 누락은 골 전송을 시도하기 전에 검출하여 조기에 FAILURE를 반환한다.

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

    // 선택 입력: 행동 트리 파일 (기본값: 빈 문자열)
    std::string behavior_tree;
    getInput("behavior_tree", behavior_tree);

    // 이후 단계 진행...
}

3.1 복합 골 파라미터의 읽기

액션 골이 다수의 필드를 포함하는 경우, 각 필드를 개별 입력 포트로부터 읽어 골 메시지에 조합한다.

// ComputePathToPose 액션의 골 구성
bool populateGoal(
    nav2_msgs::action::ComputePathToPose::Goal& goal)
{
    geometry_msgs::msg::PoseStamped start, target;

    if (!getInput("start_pose", start))
    {
        RCLCPP_ERROR(node_->get_logger(),
            "'start_pose' 누락");
        return false;
    }

    if (!getInput("goal_pose", target))
    {
        RCLCPP_ERROR(node_->get_logger(),
            "'goal_pose' 누락");
        return false;
    }

    goal.start = start;
    goal.goal = target;

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

    return true;
}

3.2 골 메시지 전체를 단일 포트로 수신

골 메시지 자체를 하나의 입력 포트로 수신하는 패턴은 블랙보드의 이전 노드가 골을 사전 구성한 경우에 적용된다.

static BT::PortsList providedPorts()
{
    return {
        BT::InputPort<nav2_msgs::action::NavigateToPose::Goal>(
            "goal_msg", "사전 구성된 골 메시지"),
        BT::InputPort<std::string>(
            "server_name", "navigate_to_pose",
            "액션 서버 이름")
    };
}

이 패턴은 골 구성 로직을 별도의 동기 액션 노드에 분리하여 단일 책임 원칙을 적용하는 경우에 유용하다.

4. 단계: 액션 서버 가용성 확인

골 전송 전에 액션 서버의 가용성을 확인한다. wait_for_action_server()는 지정된 타임아웃 동안 서버의 탐색을 시도한다.

if (!action_client_->wait_for_action_server(
        std::chrono::milliseconds(100)))
{
    RCLCPP_ERROR(node_->get_logger(),
        "액션 서버 '%s' 미가용 (100ms 대기 초과)",
        server_name.c_str());
    return BT::NodeStatus::FAILURE;
}

4.1 타임아웃의 설정 기준

서버 가용성 확인의 타임아웃은 행동 트리의 Tick 주기에 대한 영향을 고려하여 설정한다. 과도하게 긴 타임아웃은 트리의 반응성을 저하시킨다.

서버가 간헐적으로 미가용한 환경에서는 행동 트리의 상위에서 재시도 로직(Retry 데코레이터)을 적용하는 것이 서버 대기 타임아웃을 증가시키는 것보다 바람직하다.

5. 단계: 골 메시지 구성

입력 포트의 값을 ROS2 액션 골 메시지의 필드에 매핑한다.

5.1 필드 단위 매핑

auto goal_msg = nav2_msgs::action::NavigateToPose::Goal();

geometry_msgs::msg::PoseStamped pose;
getInput("goal_pose", pose);
goal_msg.pose = pose;

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

5.2 입력 포트와 골 필드의 대응 관계

골 메시지의 필드 중 기본값이 적합한 필드는 선택 입력 포트로 선언하여, XML에서의 지정을 생략할 수 있게 한다.

6. 단계: 콜백 설정 및 비동기 골 전송

SendGoalOptions 구조체를 통해 골 응답 콜백, 피드백 콜백, 결과 콜백을 설정한 후, async_send_goal()로 골을 비동기적으로 전송한다.

6.1 SendGoalOptions의 구성

auto send_goal_options =
    rclcpp_action::Client<ActionT>::SendGoalOptions();

// 골 응답 콜백: 서버가 골을 수락/거부했을 때
send_goal_options.goal_response_callback =
    [this](const%20typename%20GoalHandle::SharedPtr&%20goal_handle)
    {
        if (!goal_handle)
        {
            RCLCPP_WARN(node_->get_logger(),
                "골이 서버에 의해 거부됨");
            goal_rejected_ = true;
        }
    };

// 피드백 콜백: 서버가 피드백을 전송할 때마다
send_goal_options.feedback_callback =
    [this](typename%20GoalHandle::SharedPtr,
%20%20%20%20%20%20%20%20%20%20%20const%20std::shared_ptr<const%20Feedback>%20feedback)
    {
        latest_feedback_ = feedback;
    };

// 결과 콜백: 액션이 완료되었을 때
send_goal_options.result_callback =
    [this](const%20typename%20GoalHandle::WrappedResult&%20result)
    {
        result_ = std::make_shared<Result>(*result.result);
        result_code_ = result.code;
        goal_completed_ = true;
    };

6.2 세 가지 콜백의 호출 시점과 역할

6.3 async_send_goal() 호출

auto goal_handle_future =
    action_client_->async_send_goal(goal_msg, send_goal_options);

async_send_goal()은 std::shared_future<GoalHandle::SharedPtr>를 반환한다. 이 future가 완료되면 서버의 골 수락/거부 결과를 나타내는 골 핸들을 획득할 수 있다.

7. 단계: 골 수락 확인

골 전송 후 서버의 골 수락 응답을 확인한다. 이 확인은 onStart() 내에서 짧은 타임아웃으로 동기적으로 수행하거나, 다음 Tick에서 비동기적으로 수행하는 두 가지 접근이 존재한다.

7.1 동기적 골 수락 확인

// onStart() 내에서 골 수락을 동기적으로 확인
auto status = rclcpp::spin_until_future_complete(
    node_, goal_handle_future,
    std::chrono::milliseconds(200));

if (status != 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;
}

return BT::NodeStatus::RUNNING;

동기적 확인은 onStart()의 반환 시점에서 골의 수락 여부가 확정되므로, 이후 onRunning()에서의 상태 관리가 단순해진다.

7.2 비동기적 골 수락 확인

// onStart()에서 future를 저장하고 RUNNING 반환
goal_handle_future_ = 
    action_client_->async_send_goal(goal_msg, send_goal_options);
waiting_for_acceptance_ = true;
return BT::NodeStatus::RUNNING;

// onRunning()에서 골 수락을 비동기적으로 확인
BT::NodeStatus onRunning() override
{
    if (waiting_for_acceptance_)
    {
        if (goal_handle_future_.wait_for(
                std::chrono::milliseconds(0))
            == std::future_status::ready)
        {
            goal_handle_ = goal_handle_future_.get();
            if (!goal_handle_)
            {
                RCLCPP_ERROR(node_->get_logger(),
                    "골이 서버에 의해 거부됨");
                return BT::NodeStatus::FAILURE;
            }
            waiting_for_acceptance_ = false;
        }
        else
        {
            return BT::NodeStatus::RUNNING;
        }
    }

    // 이후 결과 확인 로직...
}

비동기적 확인은 onStart()에서 메인 스레드를 전혀 차단하지 않으나, onRunning()의 상태 관리가 복잡해진다.

7.3 두 접근의 비교

Nav2의 BtActionNode는 동기적 확인 방식을 채택하며, 골 수락 대기 타임아웃을 짧게 설정하여 메인 스레드의 차단을 최소화한다(Macenski et al., 2020).

8. 골 전송 실패의 원인과 처리

골 전송 과정에서 발생할 수 있는 실패 원인을 분류한다.

8.1 오류 코드 출력

골 전송 실패 시 오류 정보를 출력 포트를 통해 블랙보드에 기록하면, 상위 트리에서 실패 원인에 따른 분기 처리가 가능하다.

enum class GoalSendError : int
{
    INPUT_MISSING = 100,
    SERVER_UNAVAILABLE = 101,
    GOAL_REJECTED = 102,
    SEND_TIMEOUT = 103
};

// onStart() 내 실패 처리
if (!action_client_->wait_for_action_server(
        std::chrono::milliseconds(100)))
{
    setOutput("error_code",
        static_cast<int>(GoalSendError::SERVER_UNAVAILABLE));
    return BT::NodeStatus::FAILURE;
}

9. XML에서의 골 파라미터 지정

<!-- 블랙보드 키를 통한 동적 골 지정 -->
<NavigateToPose goal_pose="{target_pose}"
                 server_name="navigate_to_pose" />

<!-- 이전 노드의 출력을 골로 사용 -->
<Sequence>
    <ComputeTargetPose target="{computed_target}" />
    <NavigateToPose goal_pose="{computed_target}" />
</Sequence>

<!-- 다수의 골 파라미터 지정 -->
<ComputePathToPose start_pose="{current_pose}"
                    goal_pose="{target_pose}"
                    planner_id="GridBased" />

블랙보드 키 참조({key})를 통한 골 파라미터 지정은 노드 간 데이터 흐름을 형성하여, 트리의 유연성을 확보한다.

10. 설계 지침

1. 입력 검증의 조기 수행: 모든 필수 입력 포트의 검증을 골 전송 시도 이전에 완료하여, 불필요한 서버 통신을 방지한다.

2. 서버 가용성 확인의 짧은 타임아웃: 서버 탐색에 100–200ms의 짧은 타임아웃을 적용하여, 서버 미가용 시 트리의 반응성을 유지한다. 재시도는 트리 수준에서 처리한다.

3. 동기적 골 수락 확인의 채택: 골 수락 확인을 onStart() 내에서 동기적으로 수행하여, onRunning()의 상태 관리를 단순화한다.

4. 골 구성 로직의 분리: 복잡한 골 구성이 필요한 경우 별도의 동기 액션 노드에서 골 메시지를 사전 구성하여 블랙보드에 기록하고, 전송 노드에서 이를 읽어 사용한다.

5. 실패 원인의 구분 출력: 골 전송 실패의 원인을 오류 코드 출력 포트를 통해 블랙보드에 기록하여, 상위 트리에서의 분기 처리를 지원한다(Macenski et al., 2020; Faconti, 2022).