내비게이션 관련 조건 노드 설계 (Navigation-Related Condition Node Design)

1. 개요

내비게이션 관련 조건 노드는 로봇의 자율 이동(autonomous navigation)과 관련된 상태를 평가하여 경로 추종, 목표 도달, 경로 계획 등의 의사 결정을 지원하는 조건 노드 군(群)이다. 내비게이션 시스템은 위치 추정, 경로 계획, 경로 추종, 장애물 회피 등 복수의 하위 시스템이 상호 연계하여 동작하며, 각 하위 시스템의 상태를 조건 노드를 통해 감시하고 평가함으로써 신뢰성 높은 자율 이동을 구현한다.

2. 내비게이션 상태 감시의 범주

2.1 상태 평가 대상

내비게이션 관련 조건 노드는 다음 범주로 분류된다.

2.2 Nav2 시스템과의 연계

ROS2의 Nav2(Navigation2) 프레임워크는 행동 트리를 기반으로 내비게이션 로직을 구현한다. Nav2의 행동 트리 플러그인은 내비게이션 관련 조건 노드를 다수 포함하고 있으며, 사용자 정의 조건 노드를 추가로 등록할 수 있다. Nav2에서 제공하는 대표적인 조건 노드는 다음과 같다.

3. 공통 설계 원칙

3.1 좌표 프레임 일관성

내비게이션 관련 조건 노드에서 위치를 비교할 때, 비교 대상의 좌표 프레임이 일치하여야 한다. 목표 위치가 map 프레임에서 정의되어 있고, 현재 위치가 odom 프레임에서 추정된 경우, 두 위치를 직접 비교하면 잘못된 결과를 얻는다. tf2 라이브러리를 통해 좌표 프레임 변환을 수행하거나, 동일한 프레임에서의 위치를 사용하여야 한다.

3.2 임계값의 동적 조정

내비게이션 조건의 임계값은 운용 상황에 따라 동적으로 조정되어야 하는 경우가 있다. 예를 들어, 목표 도달 판정의 허용 오차는 주차와 같은 정밀 작업에서는 작고(예: 0.05m), 일반 이동에서는 크게(예: 0.5m) 설정된다. 블랙보드 입력 포트를 통해 임계값을 동적으로 전달하면 이러한 유연성을 구현할 수 있다.

3.3 토픽 기반 접근과 서비스 기반 접근의 선택

내비게이션 상태 정보는 대부분 토픽으로 발행되므로, 토픽 기반 조건 노드를 우선적으로 사용한다. 경로 유효성 확인과 같이 요청 시점에 동적으로 계산되어야 하는 조건에는 서비스 기반 접근이 적합하다.

4. 내비게이션 조건 노드의 아키텍처

4.1 공통 기반 클래스

내비게이션 관련 조건 노드의 공통 기반 클래스를 정의하여 tf2 변환, 좌표 프레임 검증 등의 공통 기능을 제공한다.

class NavigationConditionBase : public BT::ConditionNode
{
public:
    NavigationConditionBase(const std::string& name,
                            const BT::NodeConfiguration& config,
                            rclcpp::Node::SharedPtr node)
        : BT::ConditionNode(name, config),
          node_(node)
    {
        tf_buffer_ =
            std::make_shared<tf2_ros::Buffer>(node_->get_clock());
        tf_listener_ =
            std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
    }

protected:
    rclcpp::Node::SharedPtr node_;
    std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

    geometry_msgs::msg::PoseStamped transformPose(
        const geometry_msgs::msg::PoseStamped& input_pose,
        const std::string& target_frame)
    {
        geometry_msgs::msg::PoseStamped output_pose;
        tf_buffer_->transform(input_pose, output_pose,
                              target_frame,
                              tf2::durationFromSec(0.1));
        return output_pose;
    }
};

tf2_ros::Buffer와 tf2_ros::TransformListener를 통해 좌표 프레임 변환 기능을 제공하며, transformPose() 메서드로 임의의 프레임 간 자세 변환을 수행한다.

5. XML 행동 트리에서의 활용 패턴

5.1 내비게이션 상태 종합 감시

<BehaviorTree ID="NavigationMonitoring">
    <ReactiveSequence>
        <!-- 위치 추정 품질 확인 -->
        <Condition ID="IsLocalizationReliable"
                   topic_name="/amcl_pose"
                   max_covariance="0.25"/>
        <!-- 경로 유효성 확인 -->
        <Condition ID="IsPathValid"
                   service_name="/is_path_valid"/>
        <!-- 속도 범위 확인 -->
        <Condition ID="IsSpeedInRange"
                   topic_name="/odom"
                   max_speed="2.0"/>
        <!-- 내비게이션 수행 -->
        <Action ID="FollowPath"/>
    </ReactiveSequence>
</BehaviorTree>

5.2 목표 도달 후 후속 작업 실행

<BehaviorTree ID="NavigateAndExecute">
    <Sequence>
        <Action ID="NavigateToGoal"
                goal="{target_pose}"/>
        <Condition ID="IsGoalReached"
                   topic_name="/amcl_pose"
                   goal="{target_pose}"
                   tolerance="0.3"/>
        <Action ID="PerformTask"/>
    </Sequence>
</BehaviorTree>

5.3 내비게이션 실패 복구

<BehaviorTree ID="ResilientNavigation">
    <Fallback>
        <Sequence>
            <Condition ID="IsPathValid"
                       service_name="/is_path_valid"/>
            <Action ID="FollowPath"/>
        </Sequence>
        <Sequence>
            <Action ID="ClearCostmap"/>
            <Action ID="RecomputePath"/>
        </Sequence>
        <Action ID="SpinRecovery"/>
    </Fallback>
</BehaviorTree>

경로가 유효하면 경로를 추종하고, 유효하지 않으면 코스트맵을 초기화하고 경로를 재계산한다. 이마저도 실패하면 제자리 회전 복구를 시도한다.

6. 설계 시 고려 사항

6.1 위치 추정의 이중 구조

Nav2에서는 map → odom 변환(AMCL 등의 위치 추정)과 odom → base_link 변환(오도메트리)이 결합하여 글로벌 위치를 제공한다. 위치 추정 품질 조건 노드는 이 두 변환의 각각의 품질을 별도로 평가하거나, 최종 결합된 위치의 불확실성을 평가할 수 있다.

6.2 코스트맵 갱신 주기

글로벌 코스트맵과 로컬 코스트맵은 서로 다른 갱신 주기를 가진다. 로컬 코스트맵은 센서 데이터에 따라 빈번히 갱신되는 반면, 글로벌 코스트맵은 낮은 빈도로 갱신된다. 코스트맵 갱신 여부를 확인하는 조건 노드는 이 갱신 주기의 차이를 고려하여 평가 빈도를 설정하여야 한다.

6.3 내비게이션 복구 전략의 계층화

내비게이션 실패 시 복구 전략은 경미한 것부터 심각한 것까지 계층적으로 구성하는 것이 효과적이다. 코스트맵 초기화, 후진, 제자리 회전, 대기, 운용자 호출 등의 복구 행동을 Fallback 구조로 순차적으로 시도하며, 각 단계 사이에 조건 노드를 배치하여 복구 성공 여부를 확인한다.

7. 참고 문헌

• Colledanchise, M., & Ogren, P. (2018). Behavior Trees in Robotics and AI: An Introduction. CRC Press.
• Macenski, S., et al. (2020). “The Marathon 2: A Navigation System.” arXiv preprint arXiv:2003.00368.
• BehaviorTree.CPP 공식 문서. https://www.behaviortree.dev/
• Nav2 공식 문서. https://docs.nav2.org/