목표 위치 도달 확인 조건 노드 (Goal Position Reached Condition Node)

1. 개요

목표 위치 도달 확인 조건 노드는 로봇의 현재 위치가 지정된 목표 위치에 충분히 근접하였는지를 판정하는 조건 노드이다. 목표 도달 여부는 위치 오차(position error)와 방향 오차(orientation error)를 허용 오차(tolerance)와 비교하여 결정한다. 이 조건 노드는 웨이포인트 내비게이션, 도킹 절차, 작업 위치 도달 확인 등에 활용된다.

2. 목표 도달의 수학적 정의

2.1 위치 오차

현재 위치 \mathbf{p}_c = (x_c, y_c)와 목표 위치 \mathbf{p}_g = (x_g, y_g) 사이의 유클리드 거리(Euclidean distance) d는 다음과 같다.

d = \lVert \mathbf{p}_c - \mathbf{p}_g \rVert = \sqrt{(x_c - x_g)^2 + (y_c - y_g)^2}

3차원 공간에서는 고도 차이를 포함하여 다음과 같이 확장된다.

d_{3D} = \sqrt{(x_c - x_g)^2 + (y_c - y_g)^2 + (z_c - z_g)^2}

2.2 방향 오차

현재 방향 \psi_c와 목표 방향 \psi_g 사이의 각도 차이 \Delta\psi는 다음과 같다.

\Delta\psi = \lvert \text{atan2}(\sin(\psi_c - \psi_g), \cos(\psi_c - \psi_g)) \rvert

이 공식은 각도 랩어라운드(wraparound)를 올바르게 처리하여, 결과가 [0, \pi] 범위에 놓이도록 한다.

도달 판정 조건

\text{reached} = (d \leq d_{\text{tol}}) \wedge (\Delta\psi \leq \psi_{\text{tol}})

여기서 d_{\text{tol}}은 위치 허용 오차, \psi_{\text{tol}}은 방향 허용 오차이다. 방향 확인이 필요하지 않은 경우 \psi_{\text{tol}} = \pi로 설정하여 방향 조건을 무효화할 수 있다.

3. 토픽 기반 구현

3.1 오도메트리 기반 목표 도달 확인

class IsGoalReached
    : public BT::RosTopicSubNode<nav_msgs::msg::Odometry>
{
public:
    IsGoalReached(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>("goal_x", "목표 X 좌표 (m)"),
            BT::InputPort<double>("goal_y", "목표 Y 좌표 (m)"),
            BT::InputPort<double>("xy_tolerance", 0.3,
                "위치 허용 오차 (m)"),
            BT::InputPort<double>("yaw_tolerance", 3.14159,
                "방향 허용 오차 (rad)")
        });
    }

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

        double goal_x, goal_y, xy_tol, yaw_tol;
        getInput("goal_x", goal_x);
        getInput("goal_y", goal_y);
        getInput("xy_tolerance", xy_tol);
        getInput("yaw_tolerance", yaw_tol);

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

        if (distance > xy_tol)
        {
            return BT::NodeStatus::FAILURE;
        }

        if (yaw_tol < M_PI)
        {
            double goal_yaw;
            if (getInput("goal_yaw", goal_yaw))
            {
                double current_yaw = quaternionToYaw(
                    msg->pose.pose.orientation);
                double yaw_diff = std::abs(normalizeAngle(
                    current_yaw - goal_yaw));
                if (yaw_diff > yaw_tol)
                {
                    return BT::NodeStatus::FAILURE;
                }
            }
        }

        return BT::NodeStatus::SUCCESS;
    }

private:
    double quaternionToYaw(
        const geometry_msgs::msg::Quaternion& q) const
    {
        return std::atan2(
            2.0 * (q.w * q.z + q.x * q.y),
            1.0 - 2.0 * (q.y * q.y + q.z * q.z));
    }

    double normalizeAngle(double angle) const
    {
        return std::atan2(std::sin(angle), std::cos(angle));
    }
};

3.2 블랙보드를 통한 목표 위치 전달

목표 위치를 XML 상수가 아닌 블랙보드 키를 통해 동적으로 전달하는 방식이다.

<BehaviorTree ID="WaypointNavigation">
    <Sequence>
        <Action ID="NavigateToWaypoint"
                goal="{current_waypoint}"/>
        <Condition ID="IsGoalReached"
                   topic_name="/odom"
                   goal_x="{waypoint_x}"
                   goal_y="{waypoint_y}"
                   xy_tolerance="0.5"/>
        <Action ID="ArrivalNotification"/>
    </Sequence>
</BehaviorTree>

4. AMCL 기반 구현

보다 정확한 목표 도달 판정을 위해, 오도메트리 대신 AMCL(Adaptive Monte Carlo Localization)의 위치 추정 결과를 사용한다.

class IsGoalReachedAmcl
    : public BT::RosTopicSubNode<
          geometry_msgs::msg::PoseWithCovarianceStamped>
{
public:
    IsGoalReachedAmcl(const std::string& name,
                      const BT::NodeConfiguration& config,
                      const BT::RosNodeParams& params)
        : RosTopicSubNode(name, config, params)
    {}

    static BT::PortsList providedPorts()
    {
        return providedBasicPorts({
            BT::InputPort<geometry_msgs::msg::PoseStamped>(
                "goal", "목표 자세"),
            BT::InputPort<double>("xy_tolerance", 0.25,
                "위치 허용 오차 (m)"),
            BT::InputPort<double>("yaw_tolerance", 0.2,
                "방향 허용 오차 (rad)")
        });
    }

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

        geometry_msgs::msg::PoseStamped goal;
        getInput("goal", goal);

        double xy_tol, yaw_tol;
        getInput("xy_tolerance", xy_tol);
        getInput("yaw_tolerance", yaw_tol);

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

        if (distance > xy_tol)
        {
            return BT::NodeStatus::FAILURE;
        }

        double current_yaw = quaternionToYaw(msg->pose.pose.orientation);
        double goal_yaw = quaternionToYaw(goal.pose.orientation);
        double yaw_diff = std::abs(normalizeAngle(
            current_yaw - goal_yaw));

        if (yaw_diff > yaw_tol)
        {
            return BT::NodeStatus::FAILURE;
        }

        return BT::NodeStatus::SUCCESS;
    }

private:
    double quaternionToYaw(
        const geometry_msgs::msg::Quaternion& q) const
    {
        return std::atan2(
            2.0 * (q.w * q.z + q.x * q.y),
            1.0 - 2.0 * (q.y * q.y + q.z * q.z));
    }

    double normalizeAngle(double angle) const
    {
        return std::atan2(std::sin(angle), std::cos(angle));
    }
};

5. 설계 시 고려 사항

5.1 허용 오차의 설정 기준

허용 오차는 작업의 정밀도 요구에 따라 설정한다.

5.2 좌표 프레임 일치

현재 위치와 목표 위치가 동일한 좌표 프레임에서 정의되어야 한다. 오도메트리는 odom 프레임, AMCL은 map 프레임을 사용하므로, 목표 위치의 프레임과 일치하는 위치 소스를 선택하여야 한다.

5.3 위치 추정 불확실성 고려

위치 추정에 불확실성이 큰 경우, 허용 오차를 추정 불확실성보다 작게 설정하면 목표에 도달하였음에도 도달 판정이 이루어지지 않을 수 있다. 위치 추정의 공분산을 함께 고려하여 허용 오차를 동적으로 조정하는 방안을 검토할 수 있다.

6. 참고 문헌

• 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/