1296.46 ROS2 서비스 호출 기반 액션 노드 구현
1. 서비스 호출과 행동 트리 액션 노드의 관계
ROS2 서비스(Service)는 요청-응답(request-response) 패턴에 기반한 동기적 통신 메커니즘이다. 행동 트리에서 서비스 호출을 수행하는 액션 노드를 구현하면, 맵 초기화, 경로 계산, 파라미터 질의 등 단발성 요청에 대한 응답을 행동 트리의 제어 흐름 내에서 처리할 수 있다.
서비스 호출 기반 액션 노드는 ROS2 액션(Action)과 달리 피드백이나 진행 상태 보고가 존재하지 않으므로, 요청 전송과 응답 수신이라는 두 단계로 구성된다. 이 특성으로 인하여 서비스 호출 기반 액션 노드는 비교적 단순한 구조를 가지지만, 비동기 호출 시의 타이밍 관리와 서버 가용성 확인 등의 고려 사항이 존재한다.
2. 동기 방식 서비스 호출 액션 노드
가장 단순한 구현 방식은 BT::SyncActionNode를 상속하여 tick() 메서드 내부에서 서비스를 동기적으로 호출하는 것이다. 이 방식은 서비스 응답이 신속하게 반환되는 경우에 적합하다.
class ClearCostmapService : public BT::SyncActionNode
{
public:
ClearCostmapService(const std::string& name,
const BT::NodeConfiguration& config,
rclcpp::Node::SharedPtr node)
: BT::SyncActionNode(name, config), node_(node)
{
client_ = node_->create_client<nav2_msgs::srv::ClearEntireCostmap>(
"clear_entire_costmap");
}
BT::NodeStatus tick() override
{
if (!client_->wait_for_service(std::chrono::seconds(1))) {
return BT::NodeStatus::FAILURE;
}
auto request =
std::make_shared<nav2_msgs::srv::ClearEntireCostmap::Request>();
auto future = client_->async_send_request(request);
if (rclcpp::spin_until_future_complete(node_, future,
std::chrono::seconds(5)) !=
rclcpp::FutureReturnCode::SUCCESS)
{
return BT::NodeStatus::FAILURE;
}
return BT::NodeStatus::SUCCESS;
}
static BT::PortsList providedPorts()
{
return {};
}
private:
rclcpp::Node::SharedPtr node_;
rclcpp::Client<nav2_msgs::srv::ClearEntireCostmap>::SharedPtr client_;
};
이 구현에서 rclcpp::spin_until_future_complete()는 응답이 수신되거나 타임아웃이 발생할 때까지 현재 스레드를 차단한다. 따라서 서비스 응답 시간이 긴 경우 행동 트리의 Tick 주기를 지연시킬 수 있다.
3. 비동기 방식 서비스 호출 액션 노드
서비스 응답 시간이 불확실하거나 장시간 소요될 수 있는 경우, BT::StatefulActionNode를 상속하여 비동기 방식으로 구현하는 것이 적절하다. 이 방식에서는 onStart()에서 비동기 요청을 전송하고, onRunning()에서 응답 수신 여부를 확인한다.
class AsyncServiceCallNode : public BT::StatefulActionNode
{
public:
AsyncServiceCallNode(const std::string& name,
const BT::NodeConfiguration& config,
rclcpp::Node::SharedPtr node)
: BT::StatefulActionNode(name, config), node_(node)
{
client_ = node_->create_client<ServiceT>("service_name");
}
BT::NodeStatus onStart() override
{
if (!client_->service_is_ready()) {
return BT::NodeStatus::FAILURE;
}
auto request = std::make_shared<typename ServiceT::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) {
auto response = future_result_.get();
if (response->success) {
return BT::NodeStatus::SUCCESS;
}
return BT::NodeStatus::FAILURE;
}
return BT::NodeStatus::RUNNING;
}
void onHalted() override
{
// 서비스 호출은 취소가 불가능하므로
// 내부 상태만 초기화한다.
}
private:
rclcpp::Node::SharedPtr node_;
typename rclcpp::Client<ServiceT>::SharedPtr client_;
typename rclcpp::Client<ServiceT>::SharedFuture future_result_;
};
비동기 방식의 핵심은 onRunning()에서 wait_for(std::chrono::milliseconds(0))를 사용하여 비차단적으로 응답 수신 여부를 확인하는 것이다. 이를 통하여 행동 트리의 Tick 루프가 차단되지 않고 정상적으로 동작한다.
4. 서비스 서버 가용성 확인
서비스 호출을 수행하기 전에 서비스 서버의 가용성을 확인하는 것은 필수적이다. rclcpp::Client는 두 가지 확인 방법을 제공한다.
// 방법 1: 지정된 시간 동안 서비스 가용성 대기
bool available = client_->wait_for_service(std::chrono::seconds(1));
// 방법 2: 즉시 가용성 확인 (비차단)
bool ready = client_->service_is_ready();
동기 액션 노드에서는 wait_for_service()를 사용하여 짧은 시간 동안 대기할 수 있으나, 비동기 액션 노드의 onStart()에서는 service_is_ready()를 사용하여 즉시 가용성을 판단하고, 서비스가 준비되지 않은 경우 FAILURE를 반환하는 것이 적절하다.
5. 서비스 호출 타임아웃 처리
비동기 서비스 호출에서 서버가 응답하지 않는 상황에 대비하여 타임아웃 메커니즘을 구현하여야 한다. 요청 전송 시점의 타임스탬프를 기록하고, onRunning()에서 경과 시간을 확인하는 방식으로 구현한다.
BT::NodeStatus onStart() override
{
if (!client_->service_is_ready()) {
return BT::NodeStatus::FAILURE;
}
auto request = std::make_shared<typename ServiceT::Request>();
future_result_ = client_->async_send_request(request);
request_time_ = node_->now();
double timeout_sec;
getInput("service_timeout", timeout_sec);
timeout_duration_ = std::chrono::duration<double>(timeout_sec);
return BT::NodeStatus::RUNNING;
}
BT::NodeStatus onRunning() override
{
auto elapsed = node_->now() - request_time_;
if (elapsed.seconds() > timeout_duration_.count()) {
RCLCPP_WARN(node_->get_logger(), "서비스 호출 타임아웃이 발생하였다.");
return BT::NodeStatus::FAILURE;
}
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;
}
6. 서비스 호출과 블랙보드 연동
서비스 요청의 매개변수는 블랙보드의 입력 포트로부터 읽어오고, 응답 결과는 출력 포트를 통하여 블랙보드에 기록하는 것이 일반적인 패턴이다.
static BT::PortsList providedPorts()
{
return {
BT::InputPort<std::string>("service_name", "서비스 서버 이름"),
BT::InputPort<double>("service_timeout", 5.0, "타임아웃(초)"),
BT::InputPort<geometry_msgs::msg::PoseStamped>("start",
"시작 자세"),
BT::InputPort<geometry_msgs::msg::PoseStamped>("goal",
"목표 자세"),
BT::OutputPort<nav_msgs::msg::Path>("path", "계산된 경로")
};
}
on_tick() 또는 onStart() 콜백에서 입력 포트를 읽어 요청 메시지를 구성하고, 응답 수신 후 출력 포트에 결과를 기록한다.
BT::NodeStatus onStart() override
{
auto request = std::make_shared<ServiceT::Request>();
getInput("start", request->start);
getInput("goal", request->goal);
future_result_ = client_->async_send_request(request);
return BT::NodeStatus::RUNNING;
}
7. 서비스 호출의 제약 사항
ROS2 서비스는 액션과 달리 취소 메커니즘을 제공하지 않는다. 따라서 onHalted() 콜백에서 진행 중인 서비스 호출을 중단시킬 수 없으며, 단지 응답을 무시하고 내부 상태를 초기화하는 것만 가능하다. 이러한 제약으로 인하여 장시간 실행되는 작업에는 서비스가 아닌 액션을 사용하는 것이 권장된다.
또한, 단일 스레드 실행자(SingleThreadedExecutor) 환경에서 동기 서비스 호출을 수행할 경우, 동일 노드 내에서 서비스 서버와 클라이언트가 공존하면 교착 상태(deadlock)가 발생할 수 있다. 이를 방지하기 위하여 서비스 클라이언트 전용의 콜백 그룹(callback group)을 분리하거나, 다중 스레드 실행자(MultiThreadedExecutor)를 사용하여야 한다.
callback_group_ = node_->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive);
client_ = node_->create_client<ServiceT>(
"service_name",
rmw_qos_profile_services_default,
callback_group_);
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 기준으로 작성되었다.