1296.57 ComputePathToPose 액션 노드 구현

1. ComputePathToPose의 정의와 역할

ComputePathToPose는 Nav2 프레임워크에서 제공하는 행동 트리 액션 노드로, 로봇의 현재 위치(또는 지정된 시작 위치)로부터 목표 자세까지의 전역 경로(global path)를 계산하는 임무를 수행한다. 이 노드는 nav2_msgs::action::ComputePathToPose ROS2 액션 인터페이스의 클라이언트로 동작하며, planner_server에 경로 계획 요청을 전송한다.

ComputePathToPose는 내비게이션 파이프라인에서 경로 계획 단계를 독립적으로 실행하는 노드이다. 이 노드가 생성한 경로는 블랙보드를 통해 FollowPath 노드에 전달되어 경로 추종이 실행된다. 경로 계획과 경로 추종의 분리는 반응적 경로 갱신, 다중 계획기 전환, 경로 검증 등의 고급 내비게이션 전략을 가능하게 한다(Macenski et al., 2020).

2. 액션 인터페이스 정의

# 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

골(Goal) 필드:

• goal: 경로의 목표 자세. map 프레임에서 정의된다.
• start: 경로의 시작 자세. use_start가 true일 때에만 사용된다.
• planner_id: 사용할 경로 계획기 플러그인의 식별자. 빈 문자열일 경우 기본 계획기가 사용된다.
• use_start: true이면 start 필드의 자세를 시작 위치로 사용하고, false이면 로봇의 현재 위치를 시작 위치로 사용한다.

결과(Result) 필드:

• path: 계산된 전역 경로. nav_msgs::msg::Path 타입이다.
• planning_time: 경로 계획에 소요된 시간.

피드백(Feedback): ComputePathToPose는 피드백을 제공하지 않는다. 경로 계획은 일반적으로 수 밀리초에서 수 초 이내에 완료되므로, 실시간 진행률 보고의 필요성이 낮다.

3. 클래스 구조

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

namespace nav2_behavior_tree
{

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

  void on_tick() 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 ComputePathToPoseAction::providedPorts()
{
  return providedBasicPorts({
    BT::InputPort<geometry_msgs::msg::PoseStamped>(
      "goal", "Goal pose for path planning"),
    BT::InputPort<geometry_msgs::msg::PoseStamped>(
      "start", "Start pose (optional, uses robot pose if not set)"),
    BT::InputPort<std::string>(
      "planner_id", "",
      "Planner plugin to use"),
    BT::InputPort<bool>(
      "use_start", false,
      "Whether to use start pose instead of robot's current pose"),
    BT::OutputPort<nav_msgs::msg::Path>(
      "path", "Computed path"),
    BT::OutputPort<double>(
      "planning_time", "Time taken for path computation [s]"),
    BT::OutputPort<int>(
      "error_code", "Error code on failure")
  });
}

핵심 출력 포트는 path로, 계산된 전역 경로를 nav_msgs::msg::Path 타입으로 블랙보드에 기록한다. 이 경로는 후속 FollowPath 노드의 입력으로 사용된다.

5. on_tick() 구현

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

  bool use_start = false;
  getInput("use_start", use_start);
  goal_.use_start = use_start;

  if (use_start) {
    geometry_msgs::msg::PoseStamped start_pose;
    getInput("start", start_pose);
    goal_.start = start_pose;
  }

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

use_start 플래그가 false인 경우(기본값), planner_server는 TF2를 통해 로봇의 현재 위치를 자동으로 조회하여 경로의 시작점으로 사용한다. use_start가 true인 경우에는 명시적으로 지정된 시작 자세를 사용하며, 이는 로봇의 현재 위치가 아닌 다른 위치로부터의 경로를 계획할 필요가 있는 경우에 활용된다.

6. 결과 처리 콜백 구현

BT::NodeStatus ComputePathToPoseAction::on_success()
{
  setOutput("path", result_.result->path);
  
  double planning_time_sec =
    rclcpp::Duration(result_.result->planning_time).seconds();
  setOutput("planning_time", planning_time_sec);
  
  return BT::NodeStatus::SUCCESS;
}

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

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

on_success()에서 계산된 경로와 계획 소요 시간을 블랙보드에 기록한다. 경로는 nav_msgs::msg::Path 타입으로, header(좌표계 및 타임스탬프)와 poses(순서가 지정된 PoseStamped 배열)를 포함한다.

7. 경로 계획기 플러그인 선택

Nav2의 planner_server는 다수의 경로 계획기 플러그인을 동시에 로드할 수 있으며, planner_id를 통해 상황에 적합한 계획기를 선택한다.

행동 트리에서 상황에 따라 계획기를 전환하는 구성을 제시한다.

<Fallback>
  <!-- 먼저 빠른 계획기로 시도 -->
  <ComputePathToPose goal="{target}"
                      path="{path}"
                      planner_id="NavfnPlanner"/>
  <!-- 실패 시 정밀 계획기로 재시도 -->
  <ComputePathToPose goal="{target}"
                      path="{path}"
                      planner_id="SmacPlannerHybrid"/>
</Fallback>

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

8.1 기본 사용

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

8.2 경로 계획 실패 시 복구

<RecoveryNode number_of_retries="1">
  <ComputePathToPose goal="{target_pose}"
                      path="{path}"
                      planner_id="GridBased"/>
  <Sequence>
    <ClearEntireCostmap
      service_name="/global_costmap/clear_entirely_global_costmap"/>
    <Wait wait_duration="1"/>
  </Sequence>
</RecoveryNode>

경로 계획이 실패하는 주요 원인은 코스트맵에 장애물이 과도하게 축적되어 유효한 경로가 존재하지 않는 것이다. 코스트맵을 초기화한 후 재계획하면 최신 센서 데이터에 기반한 경로 계획이 가능하다.

8.3 주기적 경로 재계획

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

RateController 데코레이터는 경로 재계획의 빈도를 제한한다. 1Hz로 설정하면 1초마다 경로를 재계획하여 동적 환경 변화에 적응한다. 재계획 빈도가 높으면 환경 변화에 대한 반응성이 향상되지만, 계산 부하가 증가한다.

9. 시작 자세 지정

use_start 포트를 true로 설정하면 로봇의 현재 위치 대신 명시적으로 지정된 시작 자세에서 경로를 계획한다. 이 기능은 다음의 상황에서 활용된다.

• 예측적 경로 계획: 로봇이 현재 이동 중인 상태에서 다음 목적지까지의 경로를 미리 계획하는 경우, 예상 도착 자세를 시작점으로 지정한다.
• 다중 로봇 계획: 다른 로봇의 위치로부터의 경로를 계획하는 경우.
• 가상 시나리오 평가: 특정 시작 위치로부터의 경로 실현 가능성을 사전에 검증하는 경우.

<ComputePathToPose goal="{final_destination}"
                    start="{expected_arrival_pose}"
                    use_start="true"
                    path="{pre_planned_path}"
                    planner_id="GridBased"/>

10. 계획 소요 시간의 활용

planning_time 출력 포트는 경로 계획에 소요된 시간을 초 단위로 블랙보드에 기록한다. 이 정보는 다음과 같은 목적으로 활용된다.

• 성능 모니터링: 계획 소요 시간이 허용 범위를 초과하는 경우를 감지하여 시스템 과부하를 진단한다.
• 적응적 재계획 빈도 조정: 계획 소요 시간에 비례하여 재계획 주기를 동적으로 조정한다.
• 로깅 및 분석: 장기 운용에서의 계획 성능 추이를 기록하고 분석한다.

11. 경로 계획 실패의 원인

ComputePathToPose가 FAILURE를 반환하는 주요 원인을 정리한다.

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::ComputePathToPoseAction>(
          name, "compute_path_to_pose", config);
    };
  factory.registerBuilder<nav2_behavior_tree::ComputePathToPoseAction>(
    "ComputePathToPose", builder);
}

13. 성능 고려 사항

경로 계획의 계산 복잡도는 코스트맵의 크기와 해상도, 시작점과 목표점 사이의 거리, 장애물 분포에 의해 결정된다. Dijkstra 기반 계획기의 시간 복잡도는 O(V \log V)이며, 여기서 V는 코스트맵의 셀 수이다. A* 기반 계획기는 휴리스틱을 활용하여 탐색 공간을 축소하므로 실질적인 계산 시간이 감소한다.

대규모 환경에서의 경로 계획 성능을 향상시키기 위해, 코스트맵 해상도를 적절히 설정하고, 계획기의 허용 오차(tolerance) 파라미터를 조정하며, 필요 시 다해상도(multi-resolution) 계획 전략을 적용할 수 있다.

참고 문헌 및 출처

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

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