지오펜스 경계 확인 조건 노드 (Geofence Boundary Check Condition Node)

지오펜스 경계 확인 조건 노드 (Geofence Boundary Check Condition Node)

1. 개요

지오펜스 경계 확인 조건 노드는 로봇 또는 드론의 현재 위치가 사전에 정의된 지오펜스(geofence) 경계 내부에 있는지를 판정하는 조건 노드이다. 지오펜스는 가상의 지리적 경계를 의미하며, 비행 허용 구역(inclusion zone), 비행 금지 구역(exclusion zone), 작업 영역 등을 정의하는 데 사용된다. 로봇이 지오펜스 경계를 이탈하면 즉시 귀환, 정지, 착륙 등의 안전 행동을 유발하여 규제 준수와 운용 안전을 보장한다.

2. 지오펜스의 기하학적 유형

2.1 원형 지오펜스

원형 지오펜스는 중심점 (x_c, y_c)과 반경 r로 정의되며, 가장 단순한 형태이다. 현재 위치 (x, y)가 경계 내부에 있는지를 판정하는 조건은 다음과 같다.

\text{inside} = \sqrt{(x - x_c)^2 + (y - y_c)^2} \leq r

직사각형 지오펜스

직사각형 지오펜스는 축 정렬(axis-aligned) 경계로, 네 개의 좌표값 (x_{\min}, y_{\min}, x_{\max}, y_{\max})으로 정의된다.

\text{inside} = (x_{\min} \leq x \leq x_{\max}) \wedge (y_{\min} \leq y \leq y_{\max})

2.2 다각형 지오펜스

임의 형상의 지오펜스는 다각형(polygon)으로 정의되며, 꼭짓점 목록 \{(x_0, y_0), (x_1, y_1), \ldots, (x_{n-1}, y_{n-1})\}으로 표현된다. 점의 포함 여부 판정에는 레이 캐스팅(ray casting) 알고리즘을 사용한다.

2.3 GPS 좌표 기반 지오펜스

실외 환경에서는 위도(latitude)와 경도(longitude)를 기반으로 지오펜스를 정의한다. GPS 좌표 간 거리 계산에는 하버사인(Haversine) 공식을 사용한다.

d = 2R \cdot \text{asin}\left(\sqrt{\sin^2\left(\frac{\Delta\phi}{2}\right) + \cos\phi_1 \cos\phi_2 \sin^2\left(\frac{\Delta\lambda}{2}\right)}\right)

여기서 R은 지구 반경(약 6,371 km), \phi는 위도, \lambda는 경도이다.

원형 지오펜스 구현

로컬 좌표 기반

class IsInsideCircularGeofence
    : public BT::RosTopicSubNode<nav_msgs::msg::Odometry>
{
public:
    IsInsideCircularGeofence(const std::string& name,
                             const BT::NodeConfiguration& config,
                             const BT::RosNodeParams& params)
        : RosTopicSubNode(name, config, params)
    {}

    static BT::PortsList providedPorts()
    {
        return providedBasicPorts({
            BT::InputPort<double>("center_x", 0.0,
                "지오펜스 중심 X (m)"),
            BT::InputPort<double>("center_y", 0.0,
                "지오펜스 중심 Y (m)"),
            BT::InputPort<double>("radius", 50.0,
                "지오펜스 반경 (m)")
        });
    }

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

        double cx, cy, radius;
        getInput("center_x", cx);
        getInput("center_y", cy);
        getInput("radius", radius);

        double dx = msg->pose.pose.position.x - cx;
        double dy = msg->pose.pose.position.y - cy;
        double distance = std::sqrt(dx * dx + dy * dy);

        if (distance <= radius)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

GPS 좌표 기반

class IsInsideGpsGeofence
    : public BT::RosTopicSubNode<sensor_msgs::msg::NavSatFix>
{
public:
    IsInsideGpsGeofence(const std::string& name,
                        const BT::NodeConfiguration& config,
                        const BT::RosNodeParams& params)
        : RosTopicSubNode(name, config, params)
    {}

    static BT::PortsList providedPorts()
    {
        return providedBasicPorts({
            BT::InputPort<double>("center_lat", "중심 위도 (도)"),
            BT::InputPort<double>("center_lon", "중심 경도 (도)"),
            BT::InputPort<double>("radius_m", 100.0,
                "지오펜스 반경 (m)")
        });
    }

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

        if (msg->status.status <
            sensor_msgs::msg::NavSatStatus::STATUS_FIX)
        {
            return BT::NodeStatus::FAILURE;
        }

        double center_lat, center_lon, radius_m;
        getInput("center_lat", center_lat);
        getInput("center_lon", center_lon);
        getInput("radius_m", radius_m);

        double distance = haversineDistance(
            msg->latitude, msg->longitude,
            center_lat, center_lon);

        if (distance <= radius_m)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }

private:
    double haversineDistance(double lat1, double lon1,
                            double lat2, double lon2) const
    {
        constexpr double R = 6371000.0;  // 지구 반경 (m)
        double dlat = degToRad(lat2 - lat1);
        double dlon = degToRad(lon2 - lon1);
        double a = std::sin(dlat / 2.0) * std::sin(dlat / 2.0) +
                   std::cos(degToRad(lat1)) *
                   std::cos(degToRad(lat2)) *
                   std::sin(dlon / 2.0) * std::sin(dlon / 2.0);
        double c = 2.0 * std::atan2(std::sqrt(a), std::sqrt(1.0 - a));
        return R * c;
    }

    double degToRad(double deg) const
    {
        return deg * M_PI / 180.0;
    }
};

다각형 지오펜스 구현

레이 캐스팅(ray casting) 알고리즘을 사용하여 점의 다각형 내부 포함 여부를 판정한다.

class IsInsidePolygonGeofence : public BT::ConditionNode
{
public:
    IsInsidePolygonGeofence(const std::string& name,
                            const BT::NodeConfiguration& config,
                            rclcpp::Node::SharedPtr node)
        : BT::ConditionNode(name, config), node_(node)
    {
        odom_sub_ = node_->create_subscription<nav_msgs::msg::Odometry>(
            "/odom", rclcpp::QoS(1),
            [this](const%20nav_msgs::msg::Odometry::SharedPtr%20msg) {
                std::lock_guard<std::mutex> lock(mutex_);
                current_pose_ = msg;
            });
    }

    static BT::PortsList providedPorts()
    {
        return {
            BT::InputPort<std::string>("polygon",
                "다각형 꼭짓점 (x1,y1;x2,y2;...)")
        };
    }

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

        std::string polygon_str;
        getInput("polygon", polygon_str);

        auto vertices = parsePolygon(polygon_str);
        if (vertices.size() < 3)
        {
            return BT::NodeStatus::FAILURE;
        }

        double px = current_pose_->pose.pose.position.x;
        double py = current_pose_->pose.pose.position.y;

        if (isPointInPolygon(px, py, vertices))
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }

private:
    struct Point { double x, y; };

    bool isPointInPolygon(double px, double py,
                          const std::vector<Point>& poly) const
    {
        bool inside = false;
        size_t n = poly.size();
        for (size_t i = 0, j = n - 1; i < n; j = i++)
        {
            if (((poly[i].y > py) != (poly[j].y > py)) &&
                (px < (poly[j].x - poly[i].x) *
                      (py - poly[i].y) /
                      (poly[j].y - poly[i].y) + poly[i].x))
            {
                inside = !inside;
            }
        }
        return inside;
    }

    std::vector<Point> parsePolygon(const std::string& str) const
    {
        std::vector<Point> vertices;
        std::istringstream stream(str);
        std::string segment;
        while (std::getline(stream, segment, ';'))
        {
            size_t comma = segment.find(',');
            if (comma != std::string::npos)
            {
                Point p;
                p.x = std::stod(segment.substr(0, comma));
                p.y = std::stod(segment.substr(comma + 1));
                vertices.push_back(p);
            }
        }
        return vertices;
    }

    rclcpp::Node::SharedPtr node_;
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
    nav_msgs::msg::Odometry::SharedPtr current_pose_;
    std::mutex mutex_;
};

레이 캐스팅 알고리즘은 점으로부터 수평 방향으로 반직선(ray)을 발사하여, 다각형의 변과 교차하는 횟수를 센다. 교차 횟수가 홀수이면 점이 다각형 내부에 있고, 짝수이면 외부에 있다.

경계 접근 경고

지오펜스 경계에 접근하고 있는 경우를 사전에 감지하여 경고하는 조건 노드이다.

class IsApproachingGeofenceBoundary
    : public BT::RosTopicSubNode<nav_msgs::msg::Odometry>
{
public:
    IsApproachingGeofenceBoundary(
        const std::string& name,
        const BT::NodeConfiguration& config,
        const BT::RosNodeParams& params)
        : RosTopicSubNode(name, config, params)
    {}

    static BT::PortsList providedPorts()
    {
        return providedBasicPorts({
            BT::InputPort<double>("center_x", 0.0, "중심 X (m)"),
            BT::InputPort<double>("center_y", 0.0, "중심 Y (m)"),
            BT::InputPort<double>("radius", 50.0, "반경 (m)"),
            BT::InputPort<double>("warning_margin", 10.0,
                "경고 여유 거리 (m)")
        });
    }

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

        double cx, cy, radius, margin;
        getInput("center_x", cx);
        getInput("center_y", cy);
        getInput("radius", radius);
        getInput("warning_margin", margin);

        double dx = msg->pose.pose.position.x - cx;
        double dy = msg->pose.pose.position.y - cy;
        double distance = std::sqrt(dx * dx + dy * dy);

        // 경계 여유 거리 이내에 진입한 경우 SUCCESS (경고 상태)
        if (distance >= (radius - margin) && distance <= radius)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

XML 행동 트리에서의 활용

지오펜스 이탈 시 즉시 귀환

<BehaviorTree ID="GeofencedFlight">
    <ReactiveSequence>
        <Condition ID="IsInsideGpsGeofence"
                   topic_name="/gps/fix"
                   center_lat="37.5665"
                   center_lon="126.9780"
                   radius_m="500.0"/>
        <Action ID="ExecuteFlightMission"/>
    </ReactiveSequence>
</BehaviorTree>

경계 접근 경고 및 단계적 대응

<BehaviorTree ID="GeofenceWarning">
    <Fallback>
        <Sequence>
            <Inverter>
                <Condition ID="IsApproachingGeofenceBoundary"
                           topic_name="/odom"
                           center_x="0" center_y="0"
                           radius="100" warning_margin="15"/>
            </Inverter>
            <Action ID="ContinueMission"/>
        </Sequence>
        <Action ID="ReturnToCenter"/>
    </Fallback>
</BehaviorTree>

설계 시 고려 사항

GPS 정밀도와 지오펜스 여유

GPS 위치 추정에는 수 미터의 오차가 존재하므로, 지오펜스 경계에 GPS 오차를 고려한 안전 여유(safety margin)를 포함하여야 한다. 규제 경계 r_{\text{reg}}에 대해 실제 지오펜스 반경은 r_{\text{fence}} = r_{\text{reg}} - \sigma_{\text{GPS}} \times k로 설정하며, k는 신뢰도에 따른 계수이다(예: 95% 신뢰도에서 k \approx 2).

3차원 지오펜스

드론의 경우 수평 경계뿐만 아니라 고도 한계를 포함하는 3차원 지오펜스가 필요하다. 수평 경계 확인과 고도 범위 확인을 Sequence로 결합하여 3차원 지오펜스를 구현할 수 있다.

동적 지오펜스

임무 진행에 따라 지오펜스 경계가 변경되는 경우, 블랙보드를 통해 지오펜스 매개변수를 동적으로 갱신할 수 있다. 다중 로봇 환경에서는 다른 로봇의 위치에 따라 배타적 영역을 동적으로 설정하는 것도 가능하다.

참고 문헌

  • Colledanchise, M., & Ogren, P. (2018). Behavior Trees in Robotics and AI: An Introduction. CRC Press.
  • Meier, L., et al. (2015). “PX4: A Node-Based Multithreaded Open Source Robotics Framework for Deeply Embedded Platforms.” ICRA 2015.
  • BehaviorTree.CPP 공식 문서. https://www.behaviortree.dev/
  • PX4 공식 문서. https://docs.px4.io/
버전날짜변경 사항
v0.12026-04-04초안 작성