경로 유효성 확인 조건 노드 (Path Validity Check Condition Node)

경로 유효성 확인 조건 노드 (Path Validity Check Condition Node)

1. 개요

경로 유효성 확인 조건 노드는 현재 계획된 경로가 여전히 유효한지, 즉 경로 상에 장애물이 존재하지 않고 로봇이 안전하게 추종할 수 있는 상태인지를 판정하는 조건 노드이다. 동적 환경에서는 경로 계획 이후 새로운 장애물이 출현하거나 기존 장애물이 이동하여 경로가 차단될 수 있으므로, 경로의 유효성을 주기적으로 검증하는 것이 필수적이다.

2. 경로 유효성의 정의

경로 \mathcal{P} = \{\mathbf{p}_0, \mathbf{p}_1, \ldots, \mathbf{p}_N\}가 유효하려면, 경로를 구성하는 모든 웨이포인트 \mathbf{p}_i와 인접 웨이포인트 간의 선분이 장애물 영역과 교차하지 않아야 한다.

\text{valid}(\mathcal{P}) = \bigwedge_{i=0}^{N} \text{free}(\mathbf{p}_i) \wedge \bigwedge_{i=0}^{N-1} \text{collision\_free}(\overline{\mathbf{p}_i \mathbf{p}_{i+1}})

여기서 \text{free}(\mathbf{p})는 위치 \mathbf{p}가 장애물 영역에 속하지 않음을, \text{collision\_free}(\overline{\mathbf{p}_i \mathbf{p}_{i+1}})는 두 점을 잇는 선분이 장애물과 교차하지 않음을 나타낸다.

Nav2 서비스 기반 구현

Nav2는 IsPathValid 서비스를 제공하여 경로의 유효성을 코스트맵 기반으로 검증한다.

class IsCurrentPathValid : public BT::ConditionNode
{
public:
    IsCurrentPathValid(const std::string& name,
                       const BT::NodeConfiguration& config,
                       rclcpp::Node::SharedPtr node)
        : BT::ConditionNode(name, config),
          node_(node),
          cached_result_(BT::NodeStatus::FAILURE),
          has_cache_(false),
          request_pending_(false)
    {
        client_ = node_->create_client<nav2_msgs::srv::IsPathValid>(
            "/is_path_valid");
    }

    static BT::PortsList providedPorts()
    {
        return {
            BT::InputPort<nav_msgs::msg::Path>("path",
                "검증할 경로"),
            BT::InputPort<double>("cache_duration_sec", 1.0,
                "캐시 유효 기간 (초)")
        };
    }

    BT::NodeStatus tick() override
    {
        double cache_duration;
        getInput("cache_duration_sec", cache_duration);

        if (has_cache_)
        {
            auto elapsed =
                (node_->get_clock()->now() - cache_time_).seconds();
            if (elapsed < cache_duration)
            {
                return cached_result_;
            }
        }

        if (!client_->service_is_ready())
        {
            return has_cache_ ? cached_result_
                              : BT::NodeStatus::FAILURE;
        }

        if (!request_pending_)
        {
            auto request =
                std::make_shared<nav2_msgs::srv::IsPathValid::Request>();
            getInput("path", request->path);

            request_pending_ = true;
            client_->async_send_request(
                request,
                [this](rclcpp::Client<nav2_msgs::srv::IsPathValid>::
%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20SharedFuture%20future) {
                    std::lock_guard<std::mutex> lock(mutex_);
                    auto response = future.get();
                    cached_result_ = response->is_valid
                                         ? BT::NodeStatus::SUCCESS
                                         : BT::NodeStatus::FAILURE;
                    cache_time_ = node_->get_clock()->now();
                    has_cache_ = true;
                    request_pending_ = false;
                });
        }

        return has_cache_ ? cached_result_
                          : BT::NodeStatus::FAILURE;
    }

private:
    rclcpp::Node::SharedPtr node_;
    rclcpp::Client<nav2_msgs::srv::IsPathValid>::SharedPtr client_;
    std::mutex mutex_;
    BT::NodeStatus cached_result_;
    bool has_cache_;
    bool request_pending_;
    rclcpp::Time cache_time_;
};

코스트맵 기반 로컬 검증

서비스 호출 없이, 코스트맵 토픽을 구독하여 경로의 유효성을 로컬에서 검증하는 방식이다.

class IsPathClearInCostmap : public BT::ConditionNode
{
public:
    IsPathClearInCostmap(const std::string& name,
                         const BT::NodeConfiguration& config,
                         rclcpp::Node::SharedPtr node)
        : BT::ConditionNode(name, config), node_(node)
    {
        costmap_sub_ =
            node_->create_subscription<nav_msgs::msg::OccupancyGrid>(
                "/local_costmap/costmap",
                rclcpp::QoS(1),
                [this](const%20nav_msgs::msg::OccupancyGrid::SharedPtr%20msg) {
                    std::lock_guard<std::mutex> lock(mutex_);
                    costmap_ = msg;
                });
    }

    static BT::PortsList providedPorts()
    {
        return {
            BT::InputPort<nav_msgs::msg::Path>("path",
                "검증할 경로"),
            BT::InputPort<int>("max_cost", 252,
                "경로 상 최대 허용 비용")
        };
    }

    BT::NodeStatus tick() override
    {
        std::lock_guard<std::mutex> lock(mutex_);
        if (!costmap_)
        {
            return BT::NodeStatus::FAILURE;
        }

        nav_msgs::msg::Path path;
        int max_cost;
        getInput("path", path);
        getInput("max_cost", max_cost);

        for (const auto& pose : path.poses)
        {
            int cost = getCostAtPosition(
                pose.pose.position.x,
                pose.pose.position.y);
            if (cost >= max_cost || cost < 0)
            {
                return BT::NodeStatus::FAILURE;
            }
        }
        return BT::NodeStatus::SUCCESS;
    }

private:
    int getCostAtPosition(double x, double y)
    {
        double resolution = costmap_->info.resolution;
        double origin_x = costmap_->info.origin.position.x;
        double origin_y = costmap_->info.origin.position.y;

        int mx = static_cast<int>((x - origin_x) / resolution);
        int my = static_cast<int>((y - origin_y) / resolution);

        if (mx < 0 || mx >= static_cast<int>(costmap_->info.width) ||
            my < 0 || my >= static_cast<int>(costmap_->info.height))
        {
            return -1;
        }

        return costmap_->data[my * costmap_->info.width + mx];
    }

    rclcpp::Node::SharedPtr node_;
    rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr
        costmap_sub_;
    nav_msgs::msg::OccupancyGrid::SharedPtr costmap_;
    std::mutex mutex_;
};

XML 행동 트리에서의 활용

경로 유효성 감시 및 재계획

<BehaviorTree ID="ValidatedPathFollowing">
    <ReactiveSequence>
        <Condition ID="IsCurrentPathValid"
                   path="{current_path}"
                   cache_duration_sec="0.5"/>
        <Action ID="FollowPath"
                path="{current_path}"/>
    </ReactiveSequence>
</BehaviorTree>

경로가 무효화되면 ReactiveSequence에 의해 FollowPath 액션이 중단되고, 상위 제어 구조에서 경로 재계획이 실행된다.

설계 시 고려 사항

검증 범위의 한정

전체 경로를 매 tick마다 검증하는 것은 계산 비용이 높을 수 있다. 로봇의 현재 위치로부터 일정 거리(예: 전방 5m) 내의 경로 구간만을 검증하는 것이 효율적이다.

코스트맵 갱신과의 시간 정합

코스트맵이 아직 갱신되지 않은 상태에서 경로를 검증하면, 이미 제거된 장애물이 여전히 경로를 차단하는 것으로 판정될 수 있다. 코스트맵의 갱신 시점과 경로 검증 시점의 시간 차이를 최소화하여야 한다.

로봇 형상 고려

경로 웨이포인트가 자유 공간에 위치하더라도, 로봇의 물리적 크기(footprint)를 고려하면 경로 통과가 불가능할 수 있다. 경로 유효성 검증 시 로봇 형상에 따른 팽창(inflation) 비용을 반영하여야 한다.

참고 문헌

  • 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.
  • Nav2 공식 문서. https://docs.nav2.org/
  • BehaviorTree.CPP 공식 문서. https://www.behaviortree.dev/
버전날짜변경 사항
v0.12026-04-04초안 작성