1296.54 NavigateToPose 액션 노드 구현

1296.54 NavigateToPose 액션 노드 구현

1. NavigateToPose의 정의와 역할

NavigateToPose는 Nav2 프레임워크에서 제공하는 행동 트리 액션 노드로, 로봇을 지정된 단일 목표 자세(goal pose)로 이동시키는 내비게이션 임무를 실행한다. 이 노드는 nav2_msgs::action::NavigateToPose ROS2 액션 인터페이스의 클라이언트로 동작하며, bt_navigator 서버에 내비게이션 요청을 전송한다.

NavigateToPose는 내비게이션 스택의 전체 파이프라인을 단일 호출로 실행한다. 즉, 전역 경로 계획, 지역 경로 추종, 장애물 회피, 목표 도달 판정까지의 모든 과정이 서버 측에서 일괄적으로 처리된다. 이는 행동 트리에서 가장 빈번하게 사용되는 내비게이션 노드이다(Macenski et al., 2020).

2. 액션 인터페이스 정의

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

# 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

골(Goal) 필드:

  • pose: 목표 위치와 방향을 나타내는 PoseStamped 메시지로, 좌표계(frame_id)와 타임스탬프를 포함한다.
  • behavior_tree: 서버 측에서 사용할 행동 트리 XML 파일의 경로이다. 빈 문자열일 경우 서버의 기본 행동 트리가 사용된다.

피드백(Feedback) 필드:

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

3. 클래스 구조와 상속 관계

NavigateToPose 노드는 nav2_behavior_tree::BtActionNode를 상속하여 구현된다. 템플릿 매개변수로 nav2_msgs::action::NavigateToPose를 지정한다.

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

namespace nav2_behavior_tree
{

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

NavigateToPose 노드는 다음의 입출력 포트를 제공한다.

BT::PortsList NavigateToPoseAction::providedPorts()
{
  return providedBasicPorts({
    BT::InputPort<geometry_msgs::msg::PoseStamped>(
      "goal", "Destination pose"),
    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<geometry_msgs::msg::PoseStamped>(
      "current_pose", "Current robot pose from feedback"),
    BT::OutputPort<float>(
      "distance_remaining", "Remaining distance to goal")
  });
}

goal 포트는 필수 입력으로, 목표 자세를 geometry_msgs::msg::PoseStamped 타입으로 수신한다. 이 자세는 일반적으로 map 프레임에서 정의되며, 위치(position)와 방향(orientation)을 포함한다.

5. on_tick() 구현

on_tick() 콜백은 최초 Tick에서 호출되며, 블랙보드 입력 포트로부터 값을 읽어 골 메시지를 구성한다.

void NavigateToPoseAction::on_tick()
{
  geometry_msgs::msg::PoseStamped goal_pose;
  getInput("goal", goal_pose);
  goal_.pose = goal_pose;

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

goal_ 멤버 변수는 기반 클래스 BtActionNode에서 정의되며, on_tick() 완료 후 기반 클래스의 tick() 메서드가 이 골을 ROS2 액션 서버에 비동기적으로 전송한다.

6. on_wait_for_result() 구현

on_wait_for_result() 콜백은 서버에서 골이 처리되는 동안 매 Tick마다 호출되며, 최신 피드백 메시지를 매개변수로 수신한다. 이 콜백에서 피드백 정보를 블랙보드에 기록하여 행동 트리의 다른 노드에서 참조할 수 있도록 한다.

void NavigateToPoseAction::on_wait_for_result(
  std::shared_ptr<const nav2_msgs::action::NavigateToPose::Feedback>
    feedback)
{
  setOutput("current_pose", feedback->current_pose);
  setOutput("distance_remaining", feedback->distance_remaining);

  // 로그 출력
  RCLCPP_DEBUG(
    node_->get_logger(),
    "NavigateToPose: distance remaining = %.2f m, "
    "recovery count = %d",
    feedback->distance_remaining,
    feedback->number_of_recoveries);
}

피드백 정보는 다음과 같은 용도로 활용된다.

  • distance_remaining: 조건 노드에서 목표 근접 여부를 판정하는 데 사용된다.
  • number_of_recoveries: 복구 시도 횟수가 임계값을 초과하면 내비게이션을 포기하는 전략의 기준으로 활용된다.
  • current_pose: 로봇의 실시간 위치를 블랙보드에 갱신하여 다른 노드에서 활용할 수 있도록 한다.

7. 결과 처리 콜백 구현

7.1 on_success()

서버가 골을 성공적으로 완료한 경우 호출된다. 로봇이 목표 자세에 도달하였음을 의미��다.

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

7.2 on_aborted()

서버가 골 처리를 중단한 경우 호출된다. 경로를 찾을 수 없거나, 복구 행동이 모두 실패한 경우에 해당한다.

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

7.3 on_cancelled()

행동 트리의 halt 호출에 의해 골 취소가 서버에 의해 수락된 경우 호출된다.

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

취소 시 SUCCESS를 반환하는 것은, halt에 의한 중단이 오류가 아닌 정상적인 제어 흐름의 일부임을 나타낸다.

8. 플러그인 등록

NavigateToPose 노드를 BehaviorTree.CPP 팩토리에 등록하여 XML 트리 정의에서 참조 가능하도록 한다.

#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::NavigateToPoseAction>(
        name, "navigate_to_pose", config);
    };
  factory.registerBuilder<nav2_behavior_tree::NavigateToPoseAction>(
    "NavigateToPose", builder);
}

여기서 "navigate_to_pose"는 ROS2 액션 서버의 이름이다. 기본적으로 Nav2의 bt_navigator 노드가 이 이름으로 액션 서버를 제공한다.

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

9.1 기본 사용

<BehaviorTree>
  <NavigateToPose goal="{target_pose}"
                  server_name="navigate_to_pose"
                  server_timeout="10"/>
</BehaviorTree>

9.2 블랙보드를 통한 목표 자세 전달

목표 자세는 블랙보드 키를 통해 동적으로 전달된다. 상위 수준의 임무 계획 노드가 블랙보드에 목표 자세를 기록하면, NavigateToPose 노드가 이를 읽어 내비게이션을 실행한다.

<BehaviorTree>
  <Sequence>
    <!-- 임무 계획에서 목표 설정 -->
    <SetBlackboard output_key="target_pose"
                   value="{next_waypoint}"/>
    
    <!-- 내비게이션 실행 -->
    <NavigateToPose goal="{target_pose}"/>
  </Sequence>
</BehaviorTree>

9.3 복구 행동과의 통합

NavigateToPose가 실패할 경우 복구 행동을 실행하고 재시도하는 구성을 제시한다.

<BehaviorTree>
  <RetryNode num_attempts="3">
    <RecoveryNode number_of_retries="2" name="NavigateRecovery">
      <PipelineSequence>
        <RateController hz="1.0">
          <RecoveryNode number_of_retries="1">
            <ComputePathToPose goal="{goal}" path="{path}"/>
            <ClearEntireCostmap name="ClearGlobalCostmap"
                                service_name="/global_costmap/clear_entirely_global_costmap"/>
          </RecoveryNode>
        </RateController>
        <RecoveryNode number_of_retries="1">
          <FollowPath path="{path}"/>
          <ClearEntireCostmap name="ClearLocalCostmap"
                              service_name="/local_costmap/clear_entirely_local_costmap"/>
        </RecoveryNode>
      </PipelineSequence>
      <SequenceStar>
        <ClearEntireCostmap name="ClearGlobalCostmap"
                            service_name="/global_costmap/clear_entirely_global_costmap"/>
        <ClearEntireCostmap name="ClearLocalCostmap"
                            service_name="/local_costmap/clear_entirely_local_costmap"/>
        <Spin spin_dist="1.57"/>
        <Wait wait_duration="5"/>
      </SequenceStar>
    </RecoveryNode>
  </RetryNode>
</BehaviorTree>

이 구성은 Nav2의 기본 행동 트리 구조를 기반으로 하며, 다음의 복구 전략을 계층적으로 적용한다.

  1. 경로 계획 실패 시 코스트맵을 초기화하고 재계획한다.
  2. 경로 추종 실패 시 지역 코스트맵을 초기화하고 재시도한다.
  3. 상위 수준 실패 시 전역/지역 코스트맵 초기화, 제자리 회전, 대기를 순차적으로 실행한다.
  4. 최종적으로 전체 과정을 최대 3회까지 재시도한다.

10. 골 선점(Goal Preemption)

NavigateToPose 노드는 반응적 행동 트리 구성에서 골 선점을 지원한다. 기존 골이 실행 중인 상태에서 새로운 골이 입력 포트에 설정되면, 기반 클래스 BtActionNode가 기존 골을 취소하고 새로운 골을 전송한다.

이 메커니즘은 다음과 같은 상황에서 활용된다.

  • 운영자가 실시간으로 목표 지점을 변경하는 경우
  • 동적 임무 계획에 의해 목표가 갱신되는 경우
  • 장애물 발견으로 인해 대체 목표로 전환하는 경우
// BtActionNode 내부의 골 선점 로직 (개념적 표현)
void on_new_goal_received()
{
  if (goal_updated_) {
    // 기존 골 취소
    action_client_->async_cancel_goal(goal_handle_);
    // 새로운 골 전송
    send_new_goal();
    goal_updated_ = false;
  }
}

11. 좌표계 처리

NavigateToPose에 전달되는 목표 자세는 header.frame_id를 통해 좌표계를 지정한다. 서버 측에서는 TF2를 사용하여 로봇의 현재 자세와 목표 자세를 동일한 좌표계로 변환한다.

일반적으로 목표 자세는 map 프레임에서 지정하며, 이는 SLAM 또는 위치 추정 시스템이 제공하는 전역 좌표계에 해당한다. odom 프레임에서 지정할 경우 오도메트리 누적 오차에 의해 목표 위치의 정확도가 저하될 수 있으므로, 전역 위치 기반 내비게이션에서는 map 프레임을 사용하는 것이 원칙이다.

// 목표 자세 설정 예시
geometry_msgs::msg::PoseStamped goal_pose;
goal_pose.header.frame_id = "map";
goal_pose.header.stamp = node_->now();
goal_pose.pose.position.x = 3.0;
goal_pose.pose.position.y = 1.5;
goal_pose.pose.position.z = 0.0;

// 목표 방향 설정 (yaw = 90도)
tf2::Quaternion q;
q.setRPY(0.0, 0.0, M_PI / 2.0);
goal_pose.pose.orientation = tf2::toMsg(q);

12. 도착 판정 기준

NavigateToPose의 도착 판정은 서버 측의 Goal Checker 플러그인에 의해 수행된다. Nav2에서는 다음의 Goal Checker 플러그인을 제공한다.

12.1 SimpleGoalChecker

로봇의 현재 위치와 목표 위치 사이의 유클리드 거리(Euclidean distance)가 허용 오차 이내이고, 방향 차이가 허용 오차 이내일 때 도착으로 판정한다.

d = \sqrt{(x_{goal} - x_{robot})^2 + (y_{goal} - y_{robot})^2}

  • 위치 허용 오차: xy_goal_tolerance (기본값: 0.25 m)
  • 방향 허용 오차: yaw_goal_tolerance (기본값: 0.25 rad)

12.2 StoppedGoalChecker

SimpleGoalChecker의 조건에 추가하여 로봇이 정지 상태(속도가 임계값 이하)일 때에만 도착으로 판정한다. 이는 로봇이 목표 지점을 통과하여 지나가는 것을 방지한다.

도착 판정 기준은 행동 트리 노드가 아닌 서버 측 파라미터로 구성되므로, NavigateToPose 노드 자체에서는 이를 직접 제어하지 않는다. 필요한 경우 파라미터 설정 액션 노드를 사용하여 런타임에 허용 오차를 변경할 수 있다.

13. 성능 고려 사항

NavigateToPose는 내비게이션 스택의 전체 파이프라인을 단일 액션 호출로 실행하므로, 개별 경로 계획과 경로 추종을 분리하여 실행하는 방식에 비해 행동 트리의 복잡도가 감소한다. 그러나 서버 측에서 내부적으로 별도의 행동 트리를 실행하므로, 중첩된 행동 트리 실행에 따른 연산 부하를 고려하여야 한다.

대규모 환경이나 복잡한 복구 전략이 필요한 임무에서는 NavigateToPose 대신 ComputePathToPose와 FollowPath를 분리하여 사용하는 것이 행동 트리 수준에서의 제어 유연성을 제공한다.

참고 문헌 및 출처

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

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