1296.56 FollowPath 액션 노드 구현

1. FollowPath의 정의와 역할

FollowPath는 Nav2 프레임워크에서 제공하는 행동 트리 액션 노드로, 사전에 계획된 전역 경로(global path)를 따라 로봇을 이동시키는 경로 추종(path following) 임무를 실행한다. 이 노드는 nav2_msgs::action::FollowPath ROS2 액션 인터페이스의 클라이언트로 동작하며, controller_server에 경로 추종 요청을 전송한다.

FollowPath는 내비게이션 파이프라인에서 경로 계획(ComputePathToPose)과 분리된 실행 단계에 해당한다. 경로 계획 노드가 생성한 nav_msgs::msg::Path를 입력으로 받아, 제어기(controller) 플러그인이 실시간으로 속도 명령(cmd_vel)을 생성하여 로봇을 경로를 따라 이동시킨다(Macenski et al., 2020).

2. 액션 인터페이스 정의

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

# 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

골(Goal) 필드:

• path: 추종할 전역 경로. nav_msgs::msg::Path 타입으로, 순서가 지정된 PoseStamped 배열을 포함한다.
• controller_id: 사용할 제어기 플러그인의 식별자. 빈 문자열일 경우 서버의 기본 제어기가 사용된다.
• goal_checker_id: 사용할 목표 도달 판정기 플러그인의 식별자. 빈 문자열일 경우 기본 판정기가 사용된다.

피드백(Feedback) 필드:

• distance_to_goal: 경로의 최종 지점까지의 잔여 거리 [m]
• speed: 로봇의 현재 이동 속도 [m/s]

3. 클래스 구조

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

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

namespace nav2_behavior_tree
{

class FollowPathAction
  : public BtActionNode<nav2_msgs::action::FollowPath>
{
public:
  FollowPathAction(
    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::FollowPath::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 FollowPathAction::providedPorts()
{
  return providedBasicPorts({
    BT::InputPort<nav_msgs::msg::Path>(
      "path", "Path to follow"),
    BT::InputPort<std::string>(
      "controller_id", "",
      "Controller plugin to use for path following"),
    BT::InputPort<std::string>(
      "goal_checker_id", "",
      "Goal checker plugin for arrival detection"),
    BT::OutputPort<int>(
      "error_code", "Error code on failure"),
    BT::OutputPort<float>(
      "distance_to_goal", "Remaining distance to goal"),
    BT::OutputPort<float>(
      "speed", "Current robot speed")
  });
}

path 포트는 핵심 입력으로, nav_msgs::msg::Path 타입의 경로를 수신한다. 이 경로는 일반적으로 ComputePathToPose 노드가 블랙보드에 출력한 계획 결과이다.

controller_id와 goal_checker_id 포트는 사용할 플러그인을 선택하는 문자열 입력이다. Nav2의 controller_server는 다수의 제어기 플러그인을 동시에 로드할 수 있으며, 이 포트를 통해 상황에 적합한 제어기를 동적으로 선택한다.

5. on_tick() 구현

void FollowPathAction::on_tick()
{
  nav_msgs::msg::Path path;
  getInput("path", path);
  goal_.path = path;

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

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

6. on_wait_for_result() 구현

void FollowPathAction::on_wait_for_result(
  std::shared_ptr<const nav2_msgs::action::FollowPath::Feedback>
    feedback)
{
  setOutput("distance_to_goal", feedback->distance_to_goal);
  setOutput("speed", feedback->speed);

  RCLCPP_DEBUG(
    node_->get_logger(),
    "FollowPath: distance to goal = %.2f m, speed = %.2f m/s",
    feedback->distance_to_goal,
    feedback->speed);
}

피드백의 distance_to_goal은 경로의 최종 지점까지의 잔여 경로 길이를 나타내며, 행동 트리의 조건 노드에서 목표 근접 여부를 판정하는 데 활용된다. speed는 로봇의 현재 이동 속도로, 정지 상태 감지나 속도 기반 조건 평가에 사용된다.

7. 결과 처리 콜백 구현

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

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

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

서버 측에서 경로 추종이 중단(abort)되는 주요 원인은 다음과 같다.

• 장애물에 의한 경로 차단으로 안전한 속도 명령을 생성할 수 없는 경우
• 로봇이 경로로부터 과도하게 이탈한 경우
• 제어기 내부에서 유효한 속도 명령을 계산하지 못한 경우
• 서버 측 타임아웃이 초과된 경우

8. ComputePathToPose와의 연계 패턴

FollowPath는 ComputePathToPose와 함께 사용되어 경로 계획과 경로 추종을 분리하는 내비게이션 파이프라인을 구성한다.

8.1 순차 실행 패턴

경로를 한 번 계획하고 추종하는 기본 패턴이다.

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

8.2 반응적 경로 갱신 패턴

경로 추종 중 주기적으로 경로를 재계획하는 반응적 패턴이다. PipelineSequence를 사용하여 경로 계획과 경로 추종을 동시에 실행한다.

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

이 구성에서 PipelineSequence는 다음과 같이 동작한다.

1. 첫 번째 자식(경로 계획)이 SUCCESS를 반환하면 두 번째 자식(경로 추종)의 Tick을 시작한다.
2. 두 번째 자식이 RUNNING을 반환하는 동안, 첫 번째 자식도 계속 Tick된다.
3. RateController 데코레이터가 경로 계획의 실행 빈도를 1Hz로 제한한다.
4. 경로가 갱신되면 FollowPath는 새로운 경로에 대한 골 선점(preemption)을 통해 추종 경로를 자동으로 갱신한다.

8.3 복구 행동 통합 패턴

경로 추종 실패 시 코스트맵 초기화 후 재시도하는 패턴이다.

<RecoveryNode number_of_retries="1">
  <FollowPath path="{path}"
              controller_id="FollowPath"/>
  <ClearEntireCostmap
    service_name="/local_costmap/clear_entirely_local_costmap"/>
</RecoveryNode>

9. 제어기 플러그인 선택

Nav2의 controller_server는 다수의 제어기 플러그인을 동시에 로드하고, controller_id를 통해 상황에 적합한 제어기를 선택할 수 있다. 주요 제어기 플러그인은 다음과 같다.

행동 트리에서 상황에 따라 제어기를 동적으로 전환하는 사례를 제시한다.

<Fallback>
  <Sequence>
    <Condition ID="IsNarrowCorridor"/>
    <FollowPath path="{path}" controller_id="TEBController"/>
  </Sequence>
  <FollowPath path="{path}" controller_id="DWBController"/>
</Fallback>

10. 목표 도달 판정기 선택

goal_checker_id 포트를 통해 경로 추종의 완료를 판정하는 플러그인을 선택한다.

• SimpleGoalChecker: 유클리드 거리와 방향 오차가 허용 범위 이내이면 도달로 판정한다.
• StoppedGoalChecker: SimpleGoalChecker 조건에 추가하여 로봇이 정지 상태일 때에만 도달로 판정한다.

정밀한 위치 도달이 필요한 경우 StoppedGoalChecker를 사용하고, 경유점 통과 시에는 SimpleGoalChecker를 사용하여 로봇이 감속하지 않고 경유점을 부드럽게 통과할 수 있도록 구성한다.

11. 경로 갱신과 골 선점

FollowPath 노드가 RUNNING 상태에서 블랙보드의 path 값이 갱신되면, 기반 클래스 BtActionNode의 골 선점 메커니즘에 의해 기존 골이 취소되고 새로운 경로가 서버에 전송된다. 이 과정은 다음과 같이 진행된다.

1. 반응적 행동 트리 구성에서 ComputePathToPose가 새로운 경로를 블랙보드에 기록한다.
2. FollowPath의 다음 Tick에서 on_tick()이 호출되어 새로운 경로를 읽는다.
3. 새로운 경로가 이전 경로와 다른 경우 기존 골을 취소하고 새로운 골을 전송한다.
4. controller_server는 새로운 경로에 대해 경로 추종을 즉시 시작한다.

이 메커니즘은 동적 환경에서 장애물 변화에 따라 경로를 실시간으로 갱신하는 반응적 내비게이션의 핵심이다.

12. 플러그인 등록

#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::FollowPathAction>(
        name, "follow_path", config);
    };
  factory.registerBuilder<nav2_behavior_tree::FollowPathAction>(
    "FollowPath", builder);
}

13. 경로 추종 실패의 원인과 대응

FollowPath가 FAILURE를 반환하는 주요 원인과 대응 전략을 정리한다.

이러한 실패 대응은 행동 트리의 제어 노드(RecoveryNode, Fallback)를 활용하여 구성하며, FollowPath 노드 자체는 실패 원인을 error_code 출력 포트를 통해 보고하는 역할만 수행한다.

14. 성능 고려 사항

14.1 경로 크기

전역 경로의 점(point) 수가 많을수록 controller_server의 처리 부하가 증가한다. 특히 장거리 경로에서는 수천 개의 경로 점이 생성될 수 있다. Nav2의 경로 계획기는 경로 점의 해상도를 코스트맵 해상도에 맞추어 생성하므로, 코스트맵 해상도의 설정이 경로 크기와 추종 정밀도에 영향을 미친다.

14.2 제어 주기

controller_server의 제어 주기는 경로 추종의 정밀도와 반응성에 직접적인 영향을 미친다. 일반적으로 20Hz 이상의 제어 주기가 권장되며, 고속 이동 로봇이나 동적 환경에서는 더 높은 제어 주기가 필요하다. 제어 주기는 서버 측 파라미터로 구성되며, 행동 트리 노드에서 직접 제어하지 않는다.

참고 문헌 및 출처

• 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 공식 문서 — Controller Server: https://docs.nav2.org/configuration/packages/configuring-controller-server.html
• BehaviorTree.CPP 공식 문서: https://www.behaviortree.dev/

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