1296.55 NavigateThroughPoses 액션 노드 구현

1. NavigateThroughPoses의 정의와 역할

NavigateThroughPoses는 Nav2 프레임워크에서 제공하는 행동 트리 액션 노드로, 로봇을 다수의 경유점(waypoint)을 순차적으로 통과시키며 최종 목표 자세까지 이동시키는 내비게이션 임무를 실행한다. 이 노드는 nav2_msgs::action::NavigateThroughPoses ROS2 액션 인터페이스의 클라이언트로 동작한다.

NavigateToPose가 단일 목표 자세로의 이동을 수행하는 것과 달리, NavigateThroughPoses는 순서가 지정된 자세 목록을 수신하여 각 경유점을 순차적으로 통과하는 경로를 계획하고 추종한다. 이 기능은 순찰 임무, 다중 지점 물품 배송, 사전 정의된 경로 순회 등의 임무에서 필수적이다(Macenski et al., 2020).

2. 액션 인터페이스 정의

NavigateThroughPoses가 호출하는 ROS2 액션 인터페이스의 구조는 다음과 같다.

# 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

NavigateToPose 인터페이스와의 핵심적인 차이는 골 메시지의 poses 필드가 단일 자세가 아닌 자세 배열(PoseStamped[])이라는 점이다. 또한 피드백 메시지에 number_of_poses_remaining 필드가 추가되어 잔여 경유점 수를 실시간으로 보고한다.

골(Goal) 필드:

poses: 순차적으로 통과할 경유점 자세의 배열. 배열의 마지막 요소가 최종 목표 자세에 해당한다.
behavior_tree: 서버 측에서 사용할 행동 트리 XML 파일 경로.

피드백(Feedback) 필드:

current_pose: 로봇의 현재 자세
navigation_time: 내비게이션 시작 이후 경과 시간
estimated_time_remaining: 예상 잔여 시간
number_of_recoveries: 복구 행동 실행 횟수
distance_remaining: 현재 경유점까지의 잔여 거리
number_of_poses_remaining: 잔여 경유점 수

3. 클래스 구조

NavigateThroughPoses 노드는 nav2_behavior_tree::BtActionNode를 상속하여 구현된다.

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

namespace nav2_behavior_tree
{

class NavigateThroughPosesAction
  : public BtActionNode<nav2_msgs::action::NavigateThroughPoses>
{
public:
  NavigateThroughPosesAction(
    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::NavigateThroughPoses::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. 포트 정의

NavigateThroughPoses�� 입출력 포트는 경유점 배열을 수용하도록 설계된다.

BT::PortsList NavigateThroughPosesAction::providedPorts()
{
  return providedBasicPorts({
    BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
      "goals", "Ordered set of goal poses to navigate through"),
    BT::InputPort<std::string>(
      "behavior_tree", "",
      "Behavior tree XML file path on the server"),
    BT::OutputPort<int>(
      "error_code", "Error code on failure"),
    BT::OutputPort<int>(
      "number_of_poses_remaining", "Number of remaining waypoints"),
    BT::OutputPort<float>(
      "distance_remaining", "Remaining distance to current waypoint")
  });
}

goals ��트는 std::vector<geometry_msgs::msg::PoseStamped> 타입의 경유점 배열을 수신한다. 이 배열의 순서가 로봇의 방문 순서를 결정한다.

5. on_tick() 구현

on_tick()에서 블랙보드로부터 경유점 배열을 읽어 골 메시지에 설정한다.

void NavigateThroughPosesAction::on_tick()
{
  std::vector<geometry_msgs::msg::PoseStamped> goals;
  getInput("goals", goals);
  goal_.poses = goals;

  std::string behavior_tree;
  getInput("behavior_tree", behavior_tree);
  goal_.behavior_tree = behavior_tree;
}

6. on_wait_for_result() 구현

피드백 메시지에서 진행 상황을 추출하여 블랙보드에 기록한다.

void NavigateThroughPosesAction::on_wait_for_result(
  std::shared_ptr<const nav2_msgs::action::NavigateThroughPoses::Feedback>
    feedback)
{
  setOutput("number_of_poses_remaining",
    static_cast<int>(feedback->number_of_poses_remaining));
  setOutput("distance_remaining", feedback->distance_remaining);

  RCLCPP_DEBUG(
    node_->get_logger(),
    "NavigateThroughPoses: %d poses remaining, "
    "distance to next = %.2f m",
    feedback->number_of_poses_remaining,
    feedback->distance_remaining);
}

number_of_poses_remaining 피드백은 행동 트리의 조건 노드에서 경유점 진행률을 평가하는 데 활용할 수 있다. 예를 들어, 잔여 경유점 수가 특정 임계값 이하가 되면 다음 임무를 사전 준비하는 병렬 행동을 시작할 수 있다.

7. 결과 처리 콜백 구현

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

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

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

8. NavigateToPose와의 비교

구분	NavigateToPose	NavigateThroughPoses
목표 입력	단일 PoseStamped	PoseStamped 배열
경유점 지원	미지원	지원
피드백	잔여 거리, 복구 횟수	잔여 거리, 복구 횟수, 잔여 경유점 수
적합한 임무	단일 목적지 이동	순찰, 다중 지점 방문
경로 계획	단일 목표까지 계획	경유점 간 순차 계획

NavigateThroughPoses는 내부적으로 각 경유점에 대해 순차적으로 경로를 계획하고 추종한다. 현재 경유점에 도달하면 자동으로 다음 경유점을 향한 경로 계획으로 전환된��. 이 과정은 서버 측에서 자동으로 관리되므로, 클라이언트 측 행동 트리 노드에서는 별도의 경유점 전환 로직이 필요하지 않다.

9. 경유점 배열의 블랙보드 전달

경유점 배열을 블랙보드에서 관리하기 위해서는 std::vector<geometry_msgs::msg::PoseStamped> 타입의 블랙보드 항목을 사용한다.

9.1 정적 경유점 설정

사전에 정의된 경유점 목록을 임무 시작 시 블랙보드에 설정하는 방식이다.

// 행동 트리 실행 전 블랙보드 설정
std::vector<geometry_msgs::msg::PoseStamped> waypoints;

geometry_msgs::msg::PoseStamped wp1;
wp1.header.frame_id = "map";
wp1.pose.position.x = 1.0;
wp1.pose.position.y = 2.0;
wp1.pose.orientation.w = 1.0;
waypoints.push_back(wp1);

geometry_msgs::msg::PoseStamped wp2;
wp2.header.frame_id = "map";
wp2.pose.position.x = 3.0;
wp2.pose.position.y = 4.0;
wp2.pose.orientation.w = 1.0;
waypoints.push_back(wp2);

blackboard->set("patrol_waypoints", waypoints);

9.2 동적 경유점 생성

행동 트리의 다른 노드가 실행 중에 경유점 배열을 동적으로 생성하여 블랙보드에 기록하는 방식이다. 예를 들어, 탐색 대상 영역을 분석하여 최적의 순찰 경유점을 계산하는 노드가 경유점 배열을 출력할 수 있다.

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

10.1 기본 사용

<BehaviorTree>
  <NavigateThroughPoses goals="{waypoint_list}"
                         server_name="navigate_through_poses"
                         server_timeout="10"/>
</BehaviorTree>

10.2 순찰 임무 구성

무한 반복 순찰 임무를 행동 트리로 구성하는 사례를 제시한다.

<BehaviorTree>
  <KeepRunningUntilFailure>
    <Sequence>
      <NavigateThroughPoses goals="{patrol_waypoints}"/>
      <Wait wait_duration="10"/>
    </Sequence>
  </KeepRunningUntilFailure>
</BehaviorTree>

이 구성에서 KeepRunningUntilFailure 데코레이터는 자식 노드가 SUCCESS를 반환하면 자동으로 재시작하여 순찰 경로를 반복 순회하도록 한다. Wait 노드는 한 순회 완료 후 다음 순회 시작까지의 대기 시간을 부여한다.

10.3 복구 행동 통합 순찰

<BehaviorTree>
  <KeepRunningUntilFailure>
    <RecoveryNode number_of_retries="3">
      <Sequence>
        <NavigateThroughPoses goals="{patrol_waypoints}"/>
        <Wait wait_duration="5"/>
      </Sequence>
      <SequenceStar>
        <ClearEntireCostmap
          service_name="/global_costmap/clear_entirely_global_costmap"/>
        <Spin spin_dist="1.57"/>
        <Wait wait_duration="3"/>
      </SequenceStar>
    </RecoveryNode>
  </KeepRunningUntilFailure>
</BehaviorTree>

11. 경유점 도달 판정

각 경유점의 도달 판정은 서버 측의 Goal Checker 플러그인에 의해 수행된다. 경유점과 최종 목표에 대해 동일한 Goal Checker를 적용하거나, 경유점에 대해서는 완화된 허용 오차를 적용하고 최종 목표에 대해서만 엄격한 허용 오차를 적용하는 구성이 가능하다.

경유점 통과 시의 판정 기준은 다음과 같은 요소로 구성된다.

위치 허용 오차: 로봇 중심과 경유점 사이의 유클리드 거리가 허용 범위 이내인지 판정한다.
방향 허용 오차: 로봇의 현재 방향과 경유점의 목표 방향 사이의 각도 차이가 허용 범위 이내인지 판정한다. 경유점에서의 방향 정렬이 불필요한 경우 방향 허용 오차를 크게 설정한다.
정지 조건: StoppedGoalChecker를 사용할 경우 로봇이 경유점 근처에서 정지 상태인지 추가로 확인한다.

12. 경유점 순서 최적화

NavigateThroughPoses에 전달되는 경유점 배열의 순서는 로봇의 이동 효율에 직접적인 영향을 미친다. 행동 트리 외부에서 경유점 순서를 최적화하는 방법은 다음과 같다.

12.1 최근접 이웃 탐색

현재 위치로부터 가장 가까운 미방문 경유점을 다음 방문 지점으로 선택하는 탐욕적(greedy) 접근 방식이다. 구현이 단순하지만 전역 최적해를 보장하지는 않는다.

12.2 외판원 문제 근사 해법

모든 경유점을 최소 이동 거리로 방문하는 순서를 결정하는 문제는 외판원 문제(Traveling Salesman Problem, TSP)에 해당한다. 경유점 수가 적은 경우(일반적으로 20개 이하) 근사 해법을 적용하여 효율적인 방문 순서를 결정할 수 있다.

경유점 순서 최적화는 NavigateThroughPoses 노드의 책임이 아니며, 행동 트리의 상위 수준에서 별도의 계획 노드가 수행하여 최적화된 경유점 배열을 블랙보드에 기록하는 구조가 적절하다.

13. 플러그인 등록

#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::NavigateThroughPosesAction>(
          name, "navigate_through_poses", config);
    };
  factory.registerBuilder<
    nav2_behavior_tree::NavigateThroughPosesAction>(
      "NavigateThroughPoses", builder);
}

14. 중단 시의 동작

행동 트리의 제어 노드에 의해 NavigateThroughPoses가 중단(halt)되면, 기반 클래스 BtActionNode의 halt 메커니즘에 의해 ROS2 액션 서버에 골 취소 요청이 전송된다. 서버는 현재 진행 중인 내비게이션을 중단하고 로봇을 정지시킨다.

중단 후 다시 Tick이 시작되면 새로운 골이 전송되며, 이전에 통과한 경유점부터 재시작하지 않고 전체 경유점 배열에 대해 새로운 내비게이션을 개시한다. 부분 완료 상태에서의 재시작이 필요한 경우에는 행동 트리 수준에서 잔여 경유점 배열을 관리하는 별도의 로직을 구현하여야 한다.

15. 설계 시 고려 사항

15.1 대규모 경유점 배열

경유점 수가 매우 많은 경우(수백 개 이상), 전체 배열을 단일 골 메시지로 전송하는 것은 메시지 크기와 서버 측의 계획 부하 측면에서 비효율적일 수 있다. 이 경우 경유점 배열을 적절한 크기의 부분 배열로 분할하고, 각 부분 배열에 대해 순차적으로 NavigateThroughPoses를 실행하는 행동 트리 구성이 권장된다.

15.2 동적 경유점 갱신

실행 중 경유점 배열이 변경되어야 하는 경우(예: 새로운 우선순위 지점 추가), 현재 실행 중인 NavigateThroughPoses 골을 취소하고 갱신된 경유점 배열로 새로운 골을 전송하여야 한다. BtActionNode의 골 선점 메커니즘을 활용하면 이 과정이 자동으로 처리된다.

15.3 경유점별 행동 실행

각 경유점에 도달할 때마다 특정 행동(데이터 수집, 상태 보고 등)을 실행하여야 하는 경우, NavigateThroughPoses 단독으로는 이를 구현할 수 없다. 이 경우 NavigateToPose를 반복 구조 내에서 경유점별로 개별 호출하고, 각 호출 사이에 필요한 행동 노드를 배치하는 행동 트리 구성을 사용하여야 한��.

참고 문헌 및 출처

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 이상.