지도 업데이트 확인 조건 노드 (Map Update Check Condition Node)

1. 개요

지도 업데이트 확인 조건 노드는 로봇이 사용하는 환경 지도(코스트맵 또는 점유 격자 지도)가 최근에 갱신되었는지를 판정하는 조건 노드이다. 동적 환경에서 지도 데이터가 오래되면 현재 환경을 정확히 반영하지 못하여, 장애물 회피 실패나 경로 계획 오류가 발생할 수 있다. 본 조건 노드는 지도의 신선도(freshness)를 확인하여, 최신 환경 정보에 기반한 내비게이션을 보장한다.

2. 코스트맵 갱신 확인의 원리

2.1 Nav2의 코스트맵 구조

Nav2에서는 글로벌 코스트맵과 로컬 코스트맵을 별도로 관리한다.

2.2 갱신 감지 방법

지도 갱신을 감지하는 방법은 다음과 같다.

1. 타임스탬프 비교: 코스트맵 메시지의 header.stamp를 현재 시각과 비교하여 경과 시간을 판정한다.
2. 메시지 수신 시점 비교: 마지막 메시지 수신 시점으로부터의 경과 시간을 측정한다.
3. 갱신 카운터: 수신된 메시지 수를 추적하여 지정된 주기 내에 갱신이 발생하였는지를 판정한다.

3. 구현

3.1 타임스탬프 기반 갱신 확인

class IsMapUpdatedRecently
    : public BT::RosTopicSubNode<nav_msgs::msg::OccupancyGrid>
{
public:
    IsMapUpdatedRecently(const std::string& name,
                         const BT::NodeConfiguration& config,
                         const BT::RosNodeParams& params)
        : RosTopicSubNode(name, config, params),
          node_(params.nh)
    {}

    static BT::PortsList providedPorts()
    {
        return providedBasicPorts({
            BT::InputPort<double>("max_age_sec", 5.0,
                "최대 허용 지도 경과 시간 (초)")
        });
    }

    BT::NodeStatus onTick(
        const nav_msgs::msg::OccupancyGrid::SharedPtr& msg) override
    {
        if (!msg)
        {
            return BT::NodeStatus::FAILURE;
        }

        double max_age;
        getInput("max_age_sec", max_age);

        rclcpp::Time now = node_->get_clock()->now();
        rclcpp::Time map_time(msg->header.stamp);
        double age = (now - map_time).seconds();

        if (age >= 0.0 && age <= max_age)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }

private:
    rclcpp::Node::SharedPtr node_;
};

3.2 변경 감지 기반 갱신 확인

지도 데이터의 내용이 실제로 변경되었는지를 확인하는 조건 노드이다. 단순히 새 메시지가 발행되었다는 것이 아니라, 지도 내용이 이전과 다른지를 판정한다.

class HasMapChanged
    : public BT::RosTopicSubNode<nav_msgs::msg::OccupancyGrid>
{
public:
    HasMapChanged(const std::string& name,
                  const BT::NodeConfiguration& config,
                  const BT::RosNodeParams& params)
        : RosTopicSubNode(name, config, params),
          has_previous_(false),
          changed_(false)
    {}

    static BT::PortsList providedPorts()
    {
        return providedBasicPorts({});
    }

    BT::NodeStatus onTick(
        const nav_msgs::msg::OccupancyGrid::SharedPtr& msg) override
    {
        if (!msg)
        {
            return BT::NodeStatus::FAILURE;
        }

        if (!has_previous_)
        {
            previous_stamp_ = msg->header.stamp;
            has_previous_ = true;
            return BT::NodeStatus::FAILURE;
        }

        rclcpp::Time current_stamp(msg->header.stamp);
        rclcpp::Time prev_stamp(previous_stamp_);

        if (current_stamp > prev_stamp)
        {
            previous_stamp_ = msg->header.stamp;
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }

private:
    bool has_previous_;
    builtin_interfaces::msg::Time previous_stamp_;
    bool changed_;
};

4. XML 행동 트리에서의 활용

4.1 지도 갱신 후 경로 재계획

<BehaviorTree ID="MapAwareNavigation">
    <ReactiveSequence>
        <Condition ID="IsMapUpdatedRecently"
                   topic_name="/global_costmap/costmap"
                   max_age_sec="10.0"/>
        <Fallback>
            <Sequence>
                <Condition ID="HasMapChanged"
                           topic_name="/global_costmap/costmap"/>
                <Action ID="RecomputeGlobalPath"/>
            </Sequence>
            <Action ID="FollowCurrentPath"/>
        </Fallback>
    </ReactiveSequence>
</BehaviorTree>

지도가 갱신되면 글로벌 경로를 재계획하고, 갱신이 없으면 현재 경로를 계속 추종한다. 지도가 일정 시간 이상 갱신되지 않으면 전체 내비게이션이 중단된다.

5. 설계 시 고려 사항

5.1 글로벌 지도와 로컬 지도의 갱신 주기 차이

글로벌 코스트맵은 갱신 주기가 길므로, 허용 경과 시간을 넉넉히 설정하여야 한다. 반면, 로컬 코스트맵은 빈번히 갱신되므로 짧은 경과 시간을 설정할 수 있다.

5.2 대역폭 고려

대규모 코스트맵 메시지를 빈번히 구독하면 통신 대역폭을 소모한다. OccupancyGrid 메시지의 크기는 지도 해상도와 범위에 비례하며, 고해상도 대규모 지도에서는 메시지당 수 MB에 달할 수 있다. 구독 큐 깊이를 1로 설정하여 최신 메시지만 유지하는 것이 바람직하다.

6. 참고 문헌

• Macenski, S., et al. (2020). “The Marathon 2: A Navigation System.” arXiv preprint arXiv:2003.00368.
• Nav2 공식 문서. https://docs.nav2.org/
• BehaviorTree.CPP 공식 문서. https://www.behaviortree.dev/