1296.48 ROS2 서비스 응답 처리 액션 노드

1. 서비스 응답 처리의 역할

서비스 응답 처리 액션 노드는 ROS2 서비스 서버로부터 수신한 응답(Response) 메시지를 분석하고, 그 결과에 따라 행동 트리의 노드 상태(SUCCESS 또는 FAILURE)를 결정하며, 응답에 포함된 유용한 데이터를 블랙보드에 기록하는 역할을 수행한다. 서비스 요청 생성과 서비스 호출이 선행 단계에서 완료된 이후, 응답 처리는 서비스 통신의 최종 단계로서 행동 트리의 제어 흐름에 결과를 반영한다.

2. 응답 메시지의 구조와 접근

ROS2 서비스 응답 메시지는 .srv 파일의 --- 구분자 하위에 정의된 필드들로 구성된다. C++ 코드에서는 ServiceT::Response 타입을 통하여 접근한다.

# GetMap.srv
---
nav_msgs/OccupancyGrid map

비동기 서비스 호출의 경우, SharedFuture를 통하여 응답 메시지를 획득한다.

auto response = future_result_.get();

동기 서비스 호출에서는 spin_until_future_complete() 이후 동일한 방식으로 응답에 접근한다.

3. 응답 상태 코드의 해석

많은 ROS2 서비스 인터페이스는 응답 메시지에 성공 여부를 나타내는 상태 필드를 포함한다. 이 상태 필드의 형태는 서비스 인터페이스에 따라 상이하며, 대표적인 패턴은 다음과 같다.

응답 상태 코드에 따른 행동 트리 노드 상태 결정의 구현 예시는 다음과 같다.

BT::NodeStatus processResponse(
    typename rclcpp::Client<ServiceT>::SharedResponse response)
{
    if (!response) {
        RCLCPP_ERROR(node_->get_logger(), "서비스 응답이 null이다.");
        return BT::NodeStatus::FAILURE;
    }

    if (response->error_code != 0) {
        RCLCPP_WARN(node_->get_logger(),
                    "서비스 오류 코드: %d", response->error_code);
        setOutput("error_code", response->error_code);
        return BT::NodeStatus::FAILURE;
    }

    return BT::NodeStatus::SUCCESS;
}

4. 응답 데이터의 블랙보드 기록

서비스 응답에 포함된 데이터를 행동 트리의 다른 노드에서 활용하기 위하여, 출력 포트를 통하여 블랙보드에 기록한다. 이 과정에서 응답 메시지의 원시 필드를 그대로 기록하거나, 필요에 따라 변환 처리를 수행할 수 있다.

BT::NodeStatus processResponse(
    typename rclcpp::Client<ServiceT>::SharedResponse response)
{
    if (!response->path.poses.empty()) {
        setOutput("path", response->path);
        setOutput("path_length",
                  static_cast<int>(response->path.poses.size()));
        return BT::NodeStatus::SUCCESS;
    }

    RCLCPP_WARN(node_->get_logger(), "수신된 경로가 비어 있다.");
    return BT::NodeStatus::FAILURE;
}

출력 포트의 정의는 providedPorts() 정적 메서드에서 수행한다.

static BT::PortsList providedPorts()
{
    return {
        BT::OutputPort<nav_msgs::msg::Path>("path", "계산된 경로"),
        BT::OutputPort<int>("path_length", "경로 웨이포인트 수"),
        BT::OutputPort<uint16_t>("error_code", "오류 코드")
    };
}

5. 통합 서비스 호출 및 응답 처리 패턴

서비스 호출과 응답 처리를 단일 액션 노드 내에서 통합 구현하는 패턴에서, 응답 처리는 비동기 호출의 결과가 수신된 시점에서 수행된다.

class GetMapService : public BT::StatefulActionNode
{
public:
    GetMapService(const std::string& name,
                  const BT::NodeConfiguration& config,
                  rclcpp::Node::SharedPtr node)
        : BT::StatefulActionNode(name, config), node_(node)
    {
        client_ = node_->create_client<nav_msgs::srv::GetMap>("map_server/map");
    }

    BT::NodeStatus onStart() override
    {
        if (!client_->service_is_ready()) {
            return BT::NodeStatus::FAILURE;
        }
        auto request = std::make_shared<nav_msgs::srv::GetMap::Request>();
        future_result_ = client_->async_send_request(request);
        return BT::NodeStatus::RUNNING;
    }

    BT::NodeStatus onRunning() override
    {
        auto status = future_result_.wait_for(std::chrono::milliseconds(0));
        if (status == std::future_status::ready) {
            return processResponse(future_result_.get());
        }
        return BT::NodeStatus::RUNNING;
    }

    void onHalted() override {}

    static BT::PortsList providedPorts()
    {
        return {
            BT::OutputPort<nav_msgs::msg::OccupancyGrid>("map", "점유 격자 지도")
        };
    }

private:
    BT::NodeStatus processResponse(
        rclcpp::Client<nav_msgs::srv::GetMap>::SharedResponse response)
    {
        if (!response) {
            return BT::NodeStatus::FAILURE;
        }

        if (response->map.data.empty()) {
            RCLCPP_WARN(node_->get_logger(), "수신된 지도 데이터가 비어 있다.");
            return BT::NodeStatus::FAILURE;
        }

        setOutput("map", response->map);
        return BT::NodeStatus::SUCCESS;
    }

    rclcpp::Node::SharedPtr node_;
    rclcpp::Client<nav_msgs::srv::GetMap>::SharedPtr client_;
    rclcpp::Client<nav_msgs::srv::GetMap>::SharedFuture future_result_;
};

6. 응답 처리 시 예외 상황 관리

서비스 응답 처리 과정에서 발생할 수 있는 예외 상황에는 다음이 포함된다.

첫째, 서비스 서버의 비정상 종료로 인하여 응답이 수신되지 않는 경우이다. 이 경우 future.get()이 예외를 발생시킬 수 있으므로, 타임아웃 메커니즘과 결합하여 처리한다.

BT::NodeStatus onRunning() override
{
    auto elapsed = node_->now() - request_time_;
    if (elapsed.seconds() > timeout_sec_) {
        RCLCPP_ERROR(node_->get_logger(),
                     "서비스 응답 대기 타임아웃이 발생하였다.");
        return BT::NodeStatus::FAILURE;
    }

    auto status = future_result_.wait_for(std::chrono::milliseconds(0));
    if (status == std::future_status::ready) {
        try {
            auto response = future_result_.get();
            return processResponse(response);
        } catch (const std::exception& e) {
            RCLCPP_ERROR(node_->get_logger(),
                         "서비스 응답 처리 중 예외 발생: %s", e.what());
            return BT::NodeStatus::FAILURE;
        }
    }
    return BT::NodeStatus::RUNNING;
}

둘째, 응답 메시지의 데이터가 기대하는 형식이나 범위를 벗어나는 경우이다. 이에 대비하여 응답 데이터의 유효성을 검증한 후 블랙보드에 기록하여야 한다.

BT::NodeStatus processResponse(
    rclcpp::Client<ServiceT>::SharedResponse response)
{
    if (!response) {
        return BT::NodeStatus::FAILURE;
    }

    // 응답 데이터의 유효성 검증
    if (!std::isfinite(response->value) ||
        response->value < MIN_VALUE ||
        response->value > MAX_VALUE)
    {
        RCLCPP_WARN(node_->get_logger(),
                    "응답 값이 유효 범위를 벗어난다: %f", response->value);
        return BT::NodeStatus::FAILURE;
    }

    setOutput("result_value", response->value);
    return BT::NodeStatus::SUCCESS;
}

7. 응답 타입 변환과 적응

서비스 응답 메시지의 필드 타입이 블랙보드에 저장하기에 적합하지 않은 경우, 응답 처리 노드에서 타입 변환을 수행한다. 예를 들어, 응답에 포함된 시간 정보를 부동소수점 초 단위로 변환하거나, 좌표 배열을 경로 메시지로 재구성하는 작업이 이에 해당한다.

BT::NodeStatus processResponse(
    rclcpp::Client<ServiceT>::SharedResponse response)
{
    // builtin_interfaces::msg::Duration을 double 초 단위로 변환
    double duration_sec =
        response->estimated_duration.sec +
        response->estimated_duration.nanosec * 1e-9;

    setOutput("duration_seconds", duration_sec);

    // 쿼터니언을 yaw 각도로 변환
    tf2::Quaternion q;
    tf2::fromMsg(response->orientation, q);
    double roll, pitch, yaw;
    tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);

    setOutput("heading_yaw", yaw);
    return BT::NodeStatus::SUCCESS;
}

이러한 변환을 통하여 행동 트리의 다른 노드들이 도메인에 적합한 데이터 형태로 응답 결과를 활용할 수 있게 된다.

8. 참고 문헌

• Macenski, S., Foote, T., Gerkey, B., Lalancette, C., & Woodall, W. (2022). “Robot Operating System 2: Design, Architecture, and Uses in the Wild.” Science Robotics, 7(66), eabm6074.
• Colledanchise, M., & Ogren, P. (2018). Behavior Trees in Robotics and AI: An Introduction. CRC Press.
• ROS 2 Documentation. Understanding Services. Open Robotics. https://docs.ros.org/en/rolling/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Services.html
• Faconti, D., & Colledanchise, M. (2022). BehaviorTree.CPP Documentation. https://www.behaviortree.dev/

본 문서는 ROS 2 Humble Hawksbill 및 BehaviorTree.CPP v4.x 기준으로 작성되었다.