1296.53 내비게이션 관련 액션 노드 설계

1. 내비게이션 액션 노드의 개요

내비게이션 관련 액션 노드는 이동 로봇의 자율 주행 임무를 행동 트리에서 실행하기 위한 리프 노드 집합이다. 이 노드들은 목표 지점으로의 이동, 경로 계획, 경로 추종, 복구 행동 등 내비게이션 스택의 핵심 기능을 행동 트리 인터페이스로 캡슐화한다.

Nav2(Navigation2) 프레임워크는 이러한 내비게이션 액션 노드의 표준적인 구현을 제공하며, 각 노드는 Nav2의 서버 컴포넌트가 제공하는 ROS2 액션 인터페이스를 클라이언트 측에서 호출하는 구조로 설계되어 있다(Macenski et al., 2020).

2. 내비게이션 스택과 행동 트리의 관계

Nav2의 내비게이션 스택은 다수의 서버 컴포넌트로 구성되며, 각 서버는 ROS2 액션 서버를 통해 기능을 노출한다. 행동 트리는 이 액션 서버들의 클라이언트로서 동작하며, 내비게이션 임무의 실행 순서와 조건 분기를 제어한다.

[행동 트리 엔진]
    ├── NavigateToPose 노드 ──→ [bt_navigator 서버]
    ├── ComputePathToPose 노드 ──→ [planner_server]
    ├── FollowPath 노드 ──→ [controller_server]
    ├── Spin 노드 ──→ [behavior_server]
    ├── Wait 노드 ──→ [behavior_server]
    └── BackUp 노드 ──→ [behavior_server]

행동 트리 엔진은 bt_navigator 서버 내부에서 실행되며, XML로 정의된 행동 트리를 해석하여 각 노드를 순차적 또는 병렬적으로 Tick한다. 각 내비게이션 액션 노드는 대응하는 서버의 ROS2 액션 인터페이스를 호출하고, 그 결과에 따라 SUCCESS, FAILURE, 또는 RUNNING을 반환한다.

3. 내비게이션 액션 노드의 분류 체계

내비게이션 관련 액션 노드는 기능적 역할에 따라 다음과 같이 분류된다.

3.1 목표 지점 이동 노드

로봇을 지정된 목표 자세(goal pose) 또는 경유점(waypoint) 집합으로 이동시키는 노드이다. 이 유형의 노드는 내비게이션 스택의 전체 파이프라인(경로 계획 → 경로 추종 → 도착 판정)을 단일 호출로 실행한다.

• NavigateToPose: 단일 목표 자세로의 이동
• NavigateThroughPoses: 다수의 경유점을 순차적으로 통과하는 이동

3.2 경로 계획 노드

현재 위치로부터 목표 지점까지의 전역 경로(global path)를 계산하는 노드이다. 경로 계획 서버(planner_server)의 ROS2 액션 인터페이스를 호출하여 경로를 생성하고, 결과를 블랙보드에 출력한다.

• ComputePathToPose: 단일 목표 자세까지의 경로 계산

3.3 경로 추종 노드

계획된 경로를 따라 로봇을 이동시키는 노드이다. 경로 추종 서버(controller_server)의 ROS2 액션 인터페이스를 호출하여 실시간 속도 명령을 생성한다.

• FollowPath: 주어진 경로에 대한 경로 추종 실행

3.4 복구 행동 노드

내비게이션 실패 시 로봇을 복구하기 위한 행동을 실행하는 노드이다. 행동 서버(behavior_server)의 ROS2 액션 인터페이스를 호출한다.

• Spin: 제자리 회전
• Wait: 지정 시간 대기
• BackUp: 후진 이동

4. 공통 설계 패턴: BtActionNode 기반 구현

내비게이션 액션 노드는 모두 Nav2의 BtActionNode 템플릿 기반 클래스를 상속하여 구현된다. BtActionNode는 ROS2 액션 클라이언트의 생명주기 관리, 골(goal) 전송, 피드백 처리, 결과 수신, 취소 처리 등의 공통 로직을 캡슐화하므로, 각 내비게이션 노드는 다음의 가상 메서드만을 재정의하면 된다.

template<class ActionT>
class NavigationActionNode
  : public nav2_behavior_tree::BtActionNode<ActionT>
{
public:
  NavigationActionNode(
    const std::string & xml_tag_name,
    const std::string & action_name,
    const BT::NodeConfiguration & conf)
  : nav2_behavior_tree::BtActionNode<ActionT>(
      xml_tag_name, action_name, conf)
  {}

  void on_tick() override
  {
    // 파생 클래스에서 골 메시지 필드를 설정
  }

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

  BT::NodeStatus on_aborted() override
  {
    return BT::NodeStatus::FAILURE;
  }
};

5. 내비게이션 ROS2 액션 인터페이스

내비게이션 액션 노드가 호출하는 주요 ROS2 액션 인터페이스를 정리한다.

5.1 nav2_msgs::action::NavigateToPose

단일 목표 자세로의 내비게이션을 실행하는 액션이다.

# Goal
geometry_msgs/PoseStamped pose
string behavior_tree
---
# Result
std_msgs/Empty result
---
# Feedback
geometry_msgs/PoseStamped current_pose
builtin_interfaces/Duration navigation_time
builtin_interfaces/Duration estimated_time_remaining
int16 number_of_recoveries
float32 distance_remaining

5.2 nav2_msgs::action::NavigateThroughPoses

다수의 경유점을 순차적으로 통과하는 내비게이션을 실행하는 액션이다.

# Goal
geometry_msgs/PoseStamped[] poses
string behavior_tree
---
# Result
std_msgs/Empty result
---
# Feedback
geometry_msgs/PoseStamped current_pose
builtin_interfaces/Duration navigation_time
builtin_interfaces/Duration estimated_time_remaining
int16 number_of_recoveries
float32 distance_remaining
int16 number_of_poses_remaining

5.3 nav2_msgs::action::ComputePathToPose

목표 자세까지의 전역 경로를 계산하는 액션이다.

# Goal
geometry_msgs/PoseStamped goal
geometry_msgs/PoseStamped start
string planner_id
bool use_start
---
# Result
nav_msgs/Path path
builtin_interfaces/Duration planning_time
---
# Feedback

5.4 nav2_msgs::action::FollowPath

계획된 경로를 추종하는 액션이다.

# Goal
nav_msgs/Path path
string controller_id
string goal_checker_id
---
# Result
std_msgs/Empty result
---
# Feedback
float32 distance_to_goal
float32 speed

6. 입출력 포트 설계 원칙

내비게이션 액션 노드의 블랙보드 포트 설계는 다음의 원칙을 따른다.

6.1 입력 포트

• 목표 자세: geometry_msgs::msg::PoseStamped 타입으로 목표 위치와 방향을 전달한다.
• 경로: nav_msgs::msg::Path 타입으로 계획된 경로를 전달한다.
• 서버 선택: planner_id, controller_id 등 문자열 타입으로 사용할 서버 플러그인을 지정한다.
• 행동 파라미터: 거리, 각도, 시간 등 행동의 세부 특성을 지정하는 수치 파라미터이다.

6.2 출력 포트

• 계산된 경로: 경로 계획 노드에서 생성된 경로를 블랙보드에 기록한다.
• 오류 코드: 실패 원인을 나타내는 코드를 출력하여 상위 제어 노드에서 실패 유형별 복구 전략을 선택할 수 있도록 한다.

static BT::PortsList providedPorts()
{
  return providedBasicPorts({
    BT::InputPort<geometry_msgs::msg::PoseStamped>("goal",
      "destination pose"),
    BT::InputPort<std::string>("planner_id", "",
      "planner plugin to use"),
    BT::OutputPort<nav_msgs::msg::Path>("path",
      "computed path"),
    BT::OutputPort<int>("error_code",
      "navigation error code")
  });
}

7. 피드백 처리 설계

내비게이션 액션 노드는 ROS2 액션 서버로부터 주기적으로 수신되는 피드백 메시지를 활용하여 진행 상황을 모니터링한다. 피드백 정보는 다음과 같은 목적으로 활용된다.

7.1 진행률 블랙보드 기록

피드백에 포함된 잔여 거리(distance_remaining), 예상 잔여 시간(estimated_time_remaining) 등의 정보를 블랙보드에 기록하여, 행동 트리의 조건 노드에서 참조할 수 있도록 한다.

void on_wait_for_result(
  std::shared_ptr<const ActionT::Feedback> feedback) override
{
  config().blackboard->set("distance_remaining",
    feedback->distance_remaining);
  config().blackboard->set("navigation_time",
    rclcpp::Duration(feedback->navigation_time).seconds());
}

7.2 조기 중단 판단

피드백 정보에 기반하여 내비게이션을 조기에 중단할지 여부를 판단할 수 있다. 예를 들어, 복구 시도 횟수가 임계값을 초과하면 현재 내비게이션 목표를 포기하는 전략을 구현할 수 있다.

8. 오류 코드 체계

Nav2는 내비게이션 실패의 원인을 체계적으로 분류하기 위한 오류 코드 체계를 제공한다. 각 서버는 결과 메시지에 error_code 필드를 포함하며, 이를 블랙보드에 출력하여 행동 트리의 상위 레벨에서 오류 유형별 복구 전략을 선택할 수 있도록 한다.

BT::NodeStatus on_aborted() override
{
  auto result = goal_handle_->get_result();
  setOutput("error_code", static_cast<int>(result->error_code));
  return BT::NodeStatus::FAILURE;
}

9. 내비게이션 행동 트리의 구성 패턴

내비게이션 액션 노드들은 행동 트리의 제어 노드와 조합하여 다양한 내비게이션 전략을 구성한다.

9.1 기본 내비게이션 패턴

경로 계획과 경로 추종을 분리하여 순차적으로 실행하는 패턴이다.

<Sequence>
  <ComputePathToPose goal="{target_pose}"
                      path="{planned_path}"
                      planner_id="GridBased"/>
  <FollowPath path="{planned_path}"
              controller_id="FollowPath"/>
</Sequence>

9.2 복구 행동 통합 패턴

내비게이션 실패 시 복구 행동을 시도하고 재시도하는 패턴이다.

<PipelineSequence>
  <RateController hz="1.0">
    <ComputePathToPose goal="{target_pose}" path="{path}"/>
  </RateController>
  <FollowPath path="{path}"/>
  <RecoveryNode>
    <Sequence>
      <ClearEntireCostmap/>
      <Spin spin_dist="1.57"/>
    </Sequence>
  </RecoveryNode>
</PipelineSequence>

9.3 반응적 내비게이션 패턴

경로 계획을 주기적으로 갱신하면서 경로를 추종하는 반응적 패턴이다. ReactiveFallback 또는 PipelineSequence를 사용하여 경로 계획 노드와 경로 추종 노드를 동시에 실행한다.

<PipelineSequence>
  <RateController hz="1.0">
    <ComputePathToPose goal="{target_pose}" path="{path}"/>
  </RateController>
  <FollowPath path="{path}"/>
</PipelineSequence>

이 구성에서 RateController 데코레이터는 경로 계획 노드의 실행 빈도를 1Hz로 제한하여, 과도한 경로 재계획으로 인한 계산 부하를 방지한다.

10. 좌표계와 프레임 변환

내비게이션 액션 노드에서 사용되는 자세(pose)와 경로(path) 데이터는 ROS2의 TF2 프레임워크에 의해 관리되는 좌표계 체계 내에서 정의된다. 일반적으로 목표 자세는 map 프레임에서 지정되며, 로봇의 현재 자세는 odom 프레임 또는 map 프레임에서 표현된다.

내비게이션 액션 노드는 목표 자세의 header.frame_id를 통해 좌표계를 지정하며, 서버 측에서 필요한 프레임 변환을 수행한다. 따라서 클라이언트 측인 행동 트리 노드에서는 명시적인 프레임 변환을 수행할 필요가 없으나, 목표 자세의 프레임 지정이 정확하여야 한다.

11. 내비게이션 액션 노드의 설계 시 고려 사항

11.1 타임아웃 설정

각 내비게이션 액션 노드에는 적절한 타임아웃을 설정하여야 한다. 경로 계획은 수 초 이내에 완료되어야 하지만, 경로 추종은 이동 거리에 따라 수 분이 소요될 수 있다. 행동 트리의 제어 노드 수준에서 Timeout 데코레이터를 활용하거나, 액션 서버의 내부 타임아웃 메커니즘을 활용한다.

11.2 골 갱신 전략

내비게이션 중 목표 자세가 변경되는 경우에 대한 전략이 필요하다. NavigateToPose 액션 노드는 새로운 골을 전송하면 이전 골이 자동으로 대체(preempt)되므로, 반응적 행동 트리 구성에서 목표 자세의 동적 갱신이 가능하다.

11.3 복구 행동의 계층적 구성

복구 행동은 경미한 복구(코스트맵 초기화)에서 심각한 복구(후진, 회전)까지 계층적으로 구성하여, 단순한 복구 시도로 해결되지 않는 경우에만 더 강한 복구 행동을 실행하도록 설계한다. 이는 Fallback 제어 노드를 활용하여 구현한다.

참고 문헌 및 출처

• 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 공식 문서 — Behavior Trees: https://docs.nav2.org/behavior_trees/index.html
• BehaviorTree.CPP 공식 문서: https://www.behaviortree.dev/

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