1296.43 ROS2 액션 결과 처리 액션 노드

1296.43 ROS2 액션 결과 처리 액션 노드

1. 결과 처리의 개요

ROS2 액션 인터페이스에서 결과(Result)는 액션의 실행이 완료되었을 때 서버가 클라이언트에 반환하는 최종 데이터이다. 결과는 피드백과 달리 액션의 생명주기 중 단 한 번만 발생하며, 액션의 성공 완료(SUCCEEDED), 서버 측 중단(ABORTED), 또는 클라이언트 요청에 의한 취소(CANCELED) 시에 전달된다. 행동 트리 액션 노드에서 결과 처리는 ROS2 액션의 ResultCode를 행동 트리의 NodeStatus로 변환하고, 결과 메시지의 필드를 출력 포트를 통해 블랙보드에 기록하는 과정으로 구성된다(Macenski et al., 2020).

2. 결과 콜백의 등록과 수신

결과의 수신은 SendGoalOptions의 result_callback에 의해 이루어진다.

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

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

WrappedResult 구조체는 다음의 두 요소를 포함한다.

필드타입의미
coderclcpp_action::ResultCode액션 완료 상태 코드
resultActionT::Result::SharedPtr결과 메시지

result_callback은 ROS2 실행기에 의해 호출되며, 수신된 결과를 멤버 변수에 저장하고 goal_completed_ 플래그를 true로 설정한다. 이 플래그는 onRunning()에서 확인되어 결과 처리를 유발한다.

3. ResultCode와 NodeStatus의 매핑

ROS2 액션의 ResultCode를 행동 트리의 NodeStatus로 변환하는 것이 결과 처리의 핵심이다.

3.1 ResultCode 열거형

namespace rclcpp_action
{
enum class ResultCode : int8_t
{
    UNKNOWN = 0,
    SUCCEEDED = 1,
    CANCELED = 2,
    ABORTED = 3
};
}

3.2 매핑 규칙

ResultCodeNodeStatus의미결과 데이터 활용
SUCCEEDEDSUCCESS액션이 성공적으로 완료됨결과 필드 출력
ABORTEDFAILURE서버가 액션을 중단함오류 코드 출력
CANCELEDFAILURE클라이언트의 취소 요청에 의해 중단됨없음 (또는 부분 결과)
UNKNOWNFAILURE알 수 없는 상태없음

3.3 매핑 구현

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

    switch (result_code_)
    {
    case ResultCode::SUCCEEDED:
        return onSuccess(*result_);

    case ResultCode::ABORTED:
        return onAborted(*result_);

    case ResultCode::CANCELED:
        return onCanceled();

    default:
        RCLCPP_ERROR(node_->get_logger(),
            "알 수 없는 ResultCode: %d",
            static_cast<int>(result_code_));
        return BT::NodeStatus::FAILURE;
    }
}

4. 성공 결과의 처리

액션이 SUCCEEDED로 완료된 경우, 결과 메시지의 필드를 출력 포트를 통해 블랙보드에 기록한다.

BT::NodeStatus onSuccess(const Result& result)
{
    // 결과 데이터를 출력 포트에 기록
    setOutput("error_code", 0);

    // 액션 타입별 결과 처리
    onResultReceived(result);

    return BT::NodeStatus::SUCCESS;
}

4.1 액션 타입별 성공 결과 처리

4.1.1 내비게이션 결과

// NavigateToPose 결과
void onResultReceived(
    const nav2_msgs::action::NavigateToPose::Result& result)
{
    setOutput("error_code", result.error_code);
}

내비게이션 액션의 결과는 오류 코드를 포함하며, SUCCEEDED 시 오류 코드는 0이다.

4.1.2 경로 계획 결과

// ComputePathToPose 결과
void onResultReceived(
    const nav2_msgs::action::ComputePathToPose::Result& result)
{
    setOutput("path", result.path);
    setOutput("planning_time",
        result.planning_time.sec
        + result.planning_time.nanosec * 1e-9);
    setOutput("error_code", result.error_code);
}

경로 계획 액션의 결과는 계산된 경로와 계획 소요 시간을 포함한다. 이 결과는 후속 경로 추종 노드의 입력으로 사용된다.

4.1.3 물체 검출 결과

// 물체 검출 액션 결과
void onResultReceived(const DetectObject::Result& result)
{
    setOutput("object_pose", result.object_pose);
    setOutput("confidence", result.confidence);
    setOutput("object_id", result.object_id);
}

5. 실패 결과의 처리

5.1 ABORTED 처리

서버가 액션을 중단한 경우, 결과 메시지에 포함된 오류 정보를 출력 포트에 기록한다.

BT::NodeStatus onAborted(const Result& result)
{
    RCLCPP_WARN(node_->get_logger(),
        "액션이 서버에 의해 중단됨");

    // 오류 코드 출력
    setOutput("error_code", getErrorCode(result));

    // 오류 메시지 출력 (가용한 경우)
    if (!getErrorMessage(result).empty())
    {
        setOutput("error_message", getErrorMessage(result));
    }

    return BT::NodeStatus::FAILURE;
}

5.2 CANCELED 처리

취소는 클라이언트의 명시적 요청에 의해 발생하므로, 결과 데이터는 부분적이거나 유효하지 않을 수 있다.

BT::NodeStatus onCanceled()
{
    RCLCPP_INFO(node_->get_logger(),
        "액션이 취소됨");

    // 취소 시 결과 데이터는 기록하지 않거나
    // 취소 상태를 나타내는 코드만 기록
    setOutput("error_code", -1);

    return BT::NodeStatus::FAILURE;
}

5.3 오류 코드 체계

Nav2 프레임워크에서는 표준화된 오류 코드 체계를 사용한다.

오류 코드의미원인
0없음 (성공)정상 완료
100번대서버 오류서버 내부 오류
200번대계획 실패경로 계획 불가
300번대제어 실패경로 추종 실패
400번대목표 무효목표 위치 접근 불가

오류 코드를 출력 포트에 기록함으로써, 상위 트리에서 Fallback 노드를 통한 오류 유형별 복구 처리가 가능해진다.

<Fallback>
    <NavigateToPose goal="{target}" error_code="{nav_error}" />
    <Sequence>
        <IsErrorCode code="{nav_error}" expected="201" />
        <ClearCostmap />
        <NavigateToPose goal="{target}" error_code="{nav_error}" />
    </Sequence>
    <Sequence>
        <IsErrorCode code="{nav_error}" expected="301" />
        <Spin angle="1.57" />
        <NavigateToPose goal="{target}" error_code="{nav_error}" />
    </Sequence>
</Fallback>

6. 결과 출력 포트의 설계

6.1 포트 선언 패턴

static BT::PortsList providedPorts()
{
    return {
        // 입력 포트 (골 파라미터)
        BT::InputPort<geometry_msgs::msg::PoseStamped>(
            "goal", "목표 위치"),

        // 공통 결과 출력 포트
        BT::OutputPort<int>(
            "error_code", "오류 코드 (0: 성공)"),
        BT::OutputPort<std::string>(
            "error_message", "오류 메시지"),

        // 액션 특화 결과 출력 포트
        BT::OutputPort<nav_msgs::msg::Path>(
            "path", "계산된 경로"),
        BT::OutputPort<double>(
            "planning_time", "경로 계획 소요 시간 (초)")
    };
}

6.2 공통 출력 포트와 특화 출력 포트의 분리

포트 범주예시기록 조건
공통 결과error_code, error_messageSUCCESS/FAILURE 모두
성공 시 결과path, object_poseSUCCESS 시
실패 시 결과오류 상세 정보FAILURE 시

공통 결과 포트인 error_code는 SUCCESS 시 0을, FAILURE 시 구체적 오류 코드를 기록하여, 상위 트리에서 통일된 방식으로 결과를 참조할 수 있게 한다.

7. 가상 함수를 통한 결과 처리의 확장

template <typename ActionT>
class RosActionNode : public BT::StatefulActionNode
{
protected:
    // 파생 클래스에서 재정의 가능한 결과 처리 함수
    virtual BT::NodeStatus onSuccess(
        const typename ActionT::Result& result)
    {
        // 기본 구현: SUCCESS 반환
        return BT::NodeStatus::SUCCESS;
    }

    virtual BT::NodeStatus onAborted(
        const typename ActionT::Result& result)
    {
        // 기본 구현: FAILURE 반환
        return BT::NodeStatus::FAILURE;
    }

    virtual BT::NodeStatus onCanceled()
    {
        // 기본 구현: FAILURE 반환
        return BT::NodeStatus::FAILURE;
    }
};

파생 클래스는 액션 타입에 특화된 결과 처리를 구현한다.

class ComputePathAction
    : public RosActionNode<
          nav2_msgs::action::ComputePathToPose>
{
protected:
    BT::NodeStatus onSuccess(
        const nav2_msgs::action::ComputePathToPose::Result&
            result) override
    {
        setOutput("path", result.path);
        setOutput("planning_time",
            result.planning_time.sec
            + result.planning_time.nanosec * 1e-9);
        setOutput("error_code", result.error_code);
        return BT::NodeStatus::SUCCESS;
    }

    BT::NodeStatus onAborted(
        const nav2_msgs::action::ComputePathToPose::Result&
            result) override
    {
        RCLCPP_WARN(node_->get_logger(),
            "경로 계획 실패: 오류 코드 %d",
            result.error_code);
        setOutput("error_code", result.error_code);
        return BT::NodeStatus::FAILURE;
    }
};

8. onRunning()에서의 결과 확인 흐름

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

    // 결과 도착 확인
    if (!goal_completed_)
    {
        // 피드백 처리 (선택적)
        if (latest_feedback_)
            onFeedbackReceived(*latest_feedback_);

        return BT::NodeStatus::RUNNING;
    }

    // 결과 처리
    using rclcpp_action::ResultCode;

    switch (result_code_)
    {
    case ResultCode::SUCCEEDED:
        return onSuccess(*result_);

    case ResultCode::ABORTED:
        return onAborted(*result_);

    case ResultCode::CANCELED:
        return onCanceled();

    default:
        return BT::NodeStatus::FAILURE;
    }
}

goal_completed_ 플래그가 true가 되면, onRunning()은 더 이상 RUNNING을 반환하지 않고, ResultCode에 따른 최종 NodeStatus를 반환한다. 이 반환에 의해 행동 트리는 이 노드의 실행을 완료로 간주하고 다음 노드로 진행한다.

9. 결과 데이터의 블랙보드 흐름

결과 데이터가 블랙보드를 통해 후속 노드에 전달되는 흐름을 기술한다.

<Sequence>
    <!-- 경로 계획: 결과를 블랙보드에 기록 -->
    <ComputePathToPose goal="{target}"
                        path="{computed_path}"
                        error_code="{plan_error}" />

    <!-- 경로 추종: 계획된 경로를 입력으로 사용 -->
    <FollowPath path="{computed_path}"
                 error_code="{follow_error}" />
</Sequence>

이 패턴에서 ComputePathToPose의 결과 출력 path가 블랙보드 키 computed_path에 기록되고, FollowPath가 이 키를 입력으로 읽는다. Sequence 제어 노드에 의해 ComputePathToPose가 SUCCESS를 반환한 후에만 FollowPath가 실행되므로, 결과 데이터의 유효성이 보장된다.

10. 결과 처리의 타이밍

시점이벤트행동 트리 상태
T₁골 전송 (onStart)RUNNING
T₂–Tₙ피드백 수신 (onRunning)RUNNING
Tₙ₊₁결과 콜백 수신goal_completed_ = true
Tₙ₊₂onRunning()에서 결과 확인SUCCESS 또는 FAILURE

결과 콜백의 수신(Tₙ₊₁)과 onRunning()에서의 결과 확인(Tₙ₊₂) 사이에는 최대 한 Tick 주기의 지연이 존재한다. 이 지연은 행동 트리의 Tick 기반 실행 모델에 의한 불가피한 특성이다.

11. 설계 지침

  1. ResultCode별 분기의 명시적 구현: 모든 ResultCode 값에 대한 처리를 명시적으로 구현하여, 예기치 않은 상태에 의한 미정의 동작을 방지한다.

  2. 성공과 실패의 출력 포트 구분: SUCCESS 시에는 결과 데이터와 성공 코드를, FAILURE 시에는 오류 코드와 오류 메시지를 출력 포트에 기록하여, 상위 트리에서 결과 유형에 따른 분기 처리를 지원한다.

  3. 공통 오류 코드 포트의 제공: 모든 액션 노드에 error_code 출력 포트를 제공하여, 통일된 오류 처리 인터페이스를 확립한다.

  4. 가상 함수를 통한 확장: 결과 처리 로직을 가상 함수로 분리하여, 기반 클래스의 공통 로직을 재사용하면서 액션 타입별 특화 처리를 파생 클래스에서 구현한다.

  5. 결과 데이터의 유효성 전제: SUCCESS 반환 전에 모든 결과 출력 포트에 유효한 값을 기록하여, 후속 노드가 블랙보드에서 유효한 데이터를 읽을 수 있도록 보장한다(Macenski et al., 2020; Faconti, 2022).