1296.59 Wait 액션 노드 구현

1. Wait 액션 노드의 정의와 역할

Wait는 Nav2 프레임워크에서 제공하는 복구 행동(recovery behavior) 액션 노드로, 로봇을 지정된 시간 동안 정지 상태로 대기시키는 임무를 수행한다. 이 노드는 nav2_msgs::action::Wait ROS2 액션 인터페이스의 클라이언트로 동작하며, behavior_server에 대기 요청을 전송한다.

Wait 행동의 주된 목적은 내비게이션 실패 후 환경의 동적 변화를 기다리는 시간을 확보하는 것이다. 구체적으로 다음과 같은 상황에서 활용된다.

• 동적 장애물(보행자, 다른 로봇 등)이 로봇의 경로로부터 이탈할 시간을 확보한다.
• 센서 데이터가 갱신되어 코스트맵이 현재 환경을 더 정확하게 반영할 시간을 부여한다.
• 다른 시스템 컴포넌트의 초기화 또는 복구가 완료될 때까지 대기한다.

2. 액션 인터페이스 정의

# nav2_msgs/action/Wait

# Goal
builtin_interfaces/Duration time
---
# Result
builtin_interfaces/Duration total_elapsed_time
---
# Feedback
builtin_interfaces/Duration time_left

골(Goal) 필드:

• time: 대기할 시간. builtin_interfaces::msg::Duration 타입으로, 초와 나노초 단위로 지정한다.

결과(Result) 필드:

• total_elapsed_time: 실제 대기한 총 시간.

피드백(Feedback) 필드:

• time_left: 잔여 대기 시간.

3. 클래스 구조

#include "nav2_behavior_tree/bt_action_node.hpp"
#include "nav2_msgs/action/wait.hpp"

namespace nav2_behavior_tree
{

class WaitAction
  : public BtActionNode<nav2_msgs::action::Wait>
{
public:
  WaitAction(
    const std::string & xml_tag_name,
    const std::string & action_name,
    const BT::NodeConfiguration & conf);

  void on_tick() override;

  void on_wait_for_result(
    std::shared_ptr<const nav2_msgs::action::Wait::Feedback>
      feedback) override;

  BT::NodeStatus on_success() override;
  BT::NodeStatus on_aborted() override;
  BT::NodeStatus on_cancelled() override;

  static BT::PortsList providedPorts();
};

}  // namespace nav2_behavior_tree

4. 포트 정의

BT::PortsList WaitAction::providedPorts()
{
  return providedBasicPorts({
    BT::InputPort<double>(
      "wait_duration", 5.0,
      "Duration to wait [s]"),
    BT::OutputPort<int>(
      "error_code", "Error code on failure")
  });
}

wait_duration 포트는 대기 시간을 초 단위로 지정한다. 기본값은 5.0초이다.

5. on_tick() 구현

void WaitAction::on_tick()
{
  double wait_duration;
  getInput("wait_duration", wait_duration);
  
  goal_.time = rclcpp::Duration::from_seconds(wait_duration);
}

6. on_wait_for_result() 구현

void WaitAction::on_wait_for_result(
  std::shared_ptr<const nav2_msgs::action::Wait::Feedback> feedback)
{
  RCLCPP_DEBUG(
    node_->get_logger(),
    "Wait: time remaining = %.1f s",
    rclcpp::Duration(feedback->time_left).seconds());
}

7. 결과 처리 콜백 구현

BT::NodeStatus WaitAction::on_success()
{
  return BT::NodeStatus::SUCCESS;
}

BT::NodeStatus WaitAction::on_aborted()
{
  setOutput("error_code",
    static_cast<int>(result_.result->error_code));
  return BT::NodeStatus::FAILURE;
}

BT::NodeStatus WaitAction::on_cancelled()
{
  return BT::NodeStatus::SUCCESS;
}

Wait 행동은 일반적으로 실패하지 않는다. ABORTED가 반환되는 경우는 서버 내부 오류가 발생한 극히 예외적인 상황에 한정된다.

8. XML 행동 트리에서의 사용

8.1 단독 사용

<Wait wait_duration="5.0"
      server_name="wait"
      server_timeout="10"/>

8.2 복구 행동 시퀀스 내 사용

Wait는 다른 복구 행동과 조합하여 사용된다. 일반적으로 코스트맵 초기화나 제자리 회전 후 환경 변화를 기다리는 용도로 배치된다.

<SequenceStar>
  <ClearEntireCostmap
    service_name="/global_costmap/clear_entirely_global_costmap"/>
  <Spin spin_dist="1.57"/>
  <Wait wait_duration="5.0"/>
</SequenceStar>

8.3 점진적 복구 전략

Fallback 제어 노드를 활용하여 대기 시간을 점진적으로 증가시키는 복구 전략을 구성할 수 있다.

<Fallback>
  <Sequence>
    <Wait wait_duration="3.0"/>
    <ComputePathToPose goal="{target}" path="{path}"/>
  </Sequence>
  <Sequence>
    <Wait wait_duration="10.0"/>
    <ClearEntireCostmap
      service_name="/global_costmap/clear_entirely_global_costmap"/>
    <ComputePathToPose goal="{target}" path="{path}"/>
  </Sequence>
</Fallback>

9. BehaviorTree.CPP 내장 Wait와의 구분

BehaviorTree.CPP 라이브러리 자체에도 대기 기능을 제공하는 데코레이터 노드(Delay)가 존재한다. Nav2의 Wait 액션 노드와 BehaviorTree.CPP의 Delay 데코레이터는 다음과 같은 차이를 가진다.

Nav2의 Wait 액션 노드는 서버 측에서 대기 중에도 안전 관련 모니터링을 수행할 수 있으며, 행동 트리의 halt에 의해 즉시 중단이 가능하다. BehaviorTree.CPP의 Delay 데코레이터는 외부 서버 없이 순수하게 행동 트리 내부에서 지연을 구현하므로, ROS2 인프라에 대한 의존이 없다.

10. 서버 측 동작 원리

behavior_server의 Wait 플러그인은 다음과 같이 동작한다.

1. 골 수신 시 대기 시작 시각을 기록한다.
2. 매 제어 주기마다 경과 시간을 계산하고 피드백을 발행한다.
3. 대기 시간이 완료되면 SUCCESS 결과를 반환한다.
4. 취소 요청을 수신하면 즉시 대기를 중단하고 CANCELLED 결과를 반환한다.

Wait 동작 중 서버는 별도의 속도 명령을 발행하지 않는다. 로봇의 정지 상태 유지는 하위 레벨의 제어 시스템에 의해 보장된다.

11. 플러그인 등록

#include "behaviortree_cpp/bt_factory.h"

BT_REGISTER_NODES(factory)
{
  BT::NodeBuilder builder =
    [](const%20std::string%20&%20name,%20const%20BT::NodeConfiguration%20&%20config)
    {
      return std::make_unique<nav2_behavior_tree::WaitAction>(
        name, "wait", config);
    };
  factory.registerBuilder<nav2_behavior_tree::WaitAction>(
    "Wait", builder);
}

12. 설계 시 고려 사항

12.1 대기 시간의 적절한 설정

대기 시간이 너무 짧으면 환경 변화가 충분히 발생하지 않아 복구 효과가 미미하다. 반면 대기 시간이 너무 길면 임무 완수 시간이 지연된다. 환경의 동적 특성(보행자 밀도, 장애물 이동 속도 등)을 고려하여 적절한 대기 시간을 설정하여야 한다.

12.2 반응적 대기 중단

Wait 동작 중에도 행동 트리의 상위 레벨에서 조건 변화를 감지하여 대기를 조기에 중단할 수 있다. 예를 들어, 반응적 행동 트리 구성에서 경로가 사용 가능해지면 대기를 즉시 중단하고 내비게이션을 재개하는 전략이 가능하다.

참고 문헌 및 출처

• Macenski, S., Martín, F., White, R., & Clavero, J. G. (2020). “The Marathon 2: A Navigation System.” arXiv preprint arXiv:2003.00368.
• Colledanchise, M., & Ogren, P. (2018). Behavior Trees in Robotics and AI: An Introduction. CRC Press.
• Nav2 공식 문서: https://docs.nav2.org/
• BehaviorTree.CPP 공식 문서: https://www.behaviortree.dev/

버전 정보: Nav2 Humble Hawksbill (ROS2 Humble) 기준, BehaviorTree.CPP v3.8 이상.